HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/rosconsole_bridge.cpp Executable file
View File

@@ -0,0 +1,29 @@
/*
* Copyright (c) 2019, Mathias Lüdtke
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <rosconsole_bridge/bridge.h>
REGISTER_ROSCONSOLE_BRIDGE;

72
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_bridge_node.cpp Executable file
View File

@@ -0,0 +1,72 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_bridge_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
// initialize the bridge both ways.
socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver);
to_socketcan_bridge.setup();
socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
to_topic_bridge.setup(nh_param);
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

133
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic.cpp Executable file
View File

@@ -0,0 +1,133 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/string.h>
#include <can_msgs/Frame.h>
#include <string>
namespace can
{
template<> can::FrameFilterSharedPtr tofilter(const XmlRpc::XmlRpcValue &ct)
{
XmlRpc::XmlRpcValue t(ct);
try // try to read as integer
{
uint32_t id = static_cast<int>(t);
return tofilter(id);
}
catch(...) // else read as string
{
return tofilter(static_cast<std::string>(t));
}
}
} // namespace can
namespace socketcan_bridge
{
SocketCANToTopic::SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
can::DriverInterfaceSharedPtr driver)
{
can_topic_ = nh->advertise<can_msgs::Frame>("received_messages",
nh_param->param("received_messages_queue_size", 10));
driver_ = driver;
};
void SocketCANToTopic::setup()
{
// register handler for frames and state changes.
frame_listener_ = driver_->createMsgListenerM(this, &SocketCANToTopic::frameCallback);
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
};
void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector &filters)
{
frame_listener_.reset(new can::FilteredFrameListener(driver_,
std::bind(&SocketCANToTopic::frameCallback,
this,
std::placeholders::_1),
filters));
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
}
void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters)
{
setup(can::tofilters(filters));
}
void SocketCANToTopic::setup(ros::NodeHandle nh)
{
XmlRpc::XmlRpcValue filters;
if (nh.getParam("can_ids", filters)) return setup(filters);
return setup();
}
void SocketCANToTopic::frameCallback(const can::Frame& f)
{
// ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str());
if (!f.isValid())
{
ROS_ERROR("Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: %d",
f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr);
return;
}
else
{
if (f.is_error)
{
// can::tostring cannot be used for dlc > 8 frames. It causes an crash
// due to usage of boost::array for the data array. The should always work.
ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str());
}
}
can_msgs::Frame msg;
// converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg)
convertSocketCANToMessage(f, msg);
msg.header.frame_id = ""; // empty frame is the de-facto standard for no frame.
msg.header.stamp = ros::Time::now();
can_topic_.publish(msg);
};
void SocketCANToTopic::stateCallback(const can::State & s)
{
std::string err;
driver_->translateError(s.internal_error, err);
if (!s.internal_error)
{
ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
else
{
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
};
}; // namespace socketcan_bridge

69
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic_node.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_to_topic_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
to_topic_bridge.setup(nh_param);
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

91
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan.cpp Executable file
View File

@@ -0,0 +1,91 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_interface/string.h>
#include <string>
namespace socketcan_bridge
{
TopicToSocketCAN::TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
can::DriverInterfaceSharedPtr driver)
{
can_topic_ = nh->subscribe<can_msgs::Frame>("sent_messages", nh_param->param("sent_messages_queue_size", 10),
std::bind(&TopicToSocketCAN::msgCallback, this, std::placeholders::_1));
driver_ = driver;
};
void TopicToSocketCAN::setup()
{
state_listener_ = driver_->createStateListener(
std::bind(&TopicToSocketCAN::stateCallback, this, std::placeholders::_1));
};
void TopicToSocketCAN::msgCallback(const can_msgs::Frame::ConstPtr& msg)
{
// ROS_DEBUG("Message came from sent_messages topic");
// translate it to the socketcan frame type.
can_msgs::Frame m = *msg.get(); // ROS message
can::Frame f; // socketcan type
// converts the can_msgs::Frame (ROS msg) to can::Frame (socketcan.h)
convertMessageToSocketCAN(m, f);
if (!f.isValid()) // check if the id and flags are appropriate.
{
// ROS_WARN("Refusing to send invalid frame: %s.", can::tostring(f, true).c_str());
// can::tostring cannot be used for dlc > 8 frames. It causes an crash
// due to usage of boost::array for the data array. The should always work.
ROS_ERROR("Invalid frame from topic: id: %#04x, length: %d, is_extended: %d", m.id, m.dlc, m.is_extended);
return;
}
bool res = driver_->send(f);
if (!res)
{
ROS_ERROR("Failed to send message: %s.", can::tostring(f, true).c_str());
}
};
void TopicToSocketCAN::stateCallback(const can::State & s)
{
std::string err;
driver_->translateError(s.internal_error, err);
if (!s.internal_error)
{
ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
else
{
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
};
}; // namespace socketcan_bridge

69
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan_node.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "topic_to_socketcan_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver);
to_socketcan_bridge.setup();
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}