diff --git a/jackal_base/CMakeLists.txt b/jackal_base/CMakeLists.txt index 620bc6c..e7dc877 100644 --- a/jackal_base/CMakeLists.txt +++ b/jackal_base/CMakeLists.txt @@ -24,8 +24,15 @@ set_target_properties(${PROJECT_NAME}_simple_joy_node PROPERTIES OUTPUT_NAME simple_joy_node PREFIX "") add_dependencies(${PROJECT_NAME}_simple_joy_node jackal_msgs_gencpp) -install(TARGETS jackal_node ${PROJECT_NAME}_simple_joy_node - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +add_executable(sensor_frame_node src/sensor_frame_node.cpp) +target_link_libraries(sensor_frame_node ${catkin_LIBRARIES}) + +install(TARGETS + jackal_node + ${PROJECT_NAME}_simple_joy_node + sensor_frame_node + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/jackal_base/include/jackal_base/sensor_frame_node.h b/jackal_base/include/jackal_base/sensor_frame_node.h new file mode 100644 index 0000000..c2ec90d --- /dev/null +++ b/jackal_base/include/jackal_base/sensor_frame_node.h @@ -0,0 +1,83 @@ +/** + * + * \file sensor_frame_node.h + * \brief Applies namespacing to frame_ids in messages produced by the + * platform's MCU. + * \author Blake Anderson + * \copyright Copyright (c) 2021, The University of Texas at Austin. + * + * 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 University of Texas 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 University of Texas 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + */ + +#ifndef JACKAL_BASE_SENSOR_FRAME_NODE_H +#define JACKAL_BASE_SENSOR_FRAME_NODE_H + +#include +#include +#include +#include + +/** + * SensorFrameNode adds namespacing for the Frame IDs + * of sensor messages emitted by the Jackal MCU. This is + * necessary to distinguish between identically-named + * frames from different robots. + * + * Operation: + * 1) Usage: rosrun jackal_base sensor_frame_node + * 2) Subscribes to the imu/data_raw, navsat/fix, and navsat/vel + * 3) Upon receiving a message on those topics, prepends the + * namespace of this node to the header/frame_id field. + * (ex. imu_link becomes jackal_two/imu_link) + */ +class SensorFrameNode +{ + public: + + SensorFrameNode(); + + private: + + void imuCallback(sensor_msgs::Imu imu_raw); + + void navSatFixCallback(sensor_msgs::NavSatFix navsat_fix_raw); + + void navSatVelCallback(geometry_msgs::TwistStamped navsat_vel_raw); + + void addNamespaceToFrameID(std::string *frame_id); + + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + + ros::Subscriber imu_sub_; + ros::Subscriber navsat_fix_sub_; + ros::Subscriber navsat_vel_sub_; + + ros::Publisher imu_pub_; + ros::Publisher navsat_fix_pub_; + ros::Publisher navsat_vel_pub_; +}; + +#endif // JACKAL_BASE_SENSOR_FRAME_NODE_H diff --git a/jackal_base/launch/base.launch b/jackal_base/launch/base.launch index 1c8acf4..23f0e5b 100644 --- a/jackal_base/launch/base.launch +++ b/jackal_base/launch/base.launch @@ -26,6 +26,9 @@ + + + diff --git a/jackal_base/src/sensor_frame_node.cpp b/jackal_base/src/sensor_frame_node.cpp new file mode 100644 index 0000000..13f37c0 --- /dev/null +++ b/jackal_base/src/sensor_frame_node.cpp @@ -0,0 +1,96 @@ +/** + * + * \file sensor_frame_node.cpp + * \brief Applies namespacing to frame_ids in messages produced by the + * platform's MCU. + * \author Blake Anderson + * \copyright Copyright (c) 2021, The University of Texas at Austin. + * + * 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 University of Texas 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 University of Texas 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. + * + * Please send comments, questions, or patches to code@clearpathrobotics.com + */ + +#include + +SensorFrameNode::SensorFrameNode() : + nh_private_("~") +{ + imu_sub_ = nh_private_.subscribe("imu/data_raw", 10, + &SensorFrameNode::imuCallback, this); + navsat_fix_sub_ = nh_private_.subscribe("navsat/fix", 10, + &SensorFrameNode::navSatFixCallback, this); + navsat_vel_sub_ = nh_private_.subscribe("navsat/vel", 10, + &SensorFrameNode::navSatVelCallback, this); + + imu_pub_ = nh_private_.advertise("imu/data", 10); + navsat_fix_pub_ = nh_private_.advertise("navsat/fix", 10); + navsat_vel_pub_ = nh_private_.advertise("navsat/vel", 10); +} + + +void SensorFrameNode::imuCallback(sensor_msgs::Imu imu_raw) +{ + addNamespaceToFrameID(&imu_raw.header.frame_id); + + imu_pub_.publish(imu_raw); +} + + +void SensorFrameNode::navSatFixCallback(sensor_msgs::NavSatFix navsat_fix_raw) +{ + addNamespaceToFrameID(&navsat_fix_raw.header.frame_id); + + navsat_fix_pub_.publish(navsat_fix_raw); +} + + +void SensorFrameNode::navSatVelCallback(geometry_msgs::TwistStamped navsat_vel_raw) +{ + addNamespaceToFrameID(&navsat_vel_raw.header.frame_id); + + navsat_vel_pub_.publish(navsat_vel_raw); +} + + +void SensorFrameNode::addNamespaceToFrameID(std::string *frame_id) +{ + assert(frame_id); + + const std::string ns = nh_private_.getNamespace(); + + if (!ns.empty()) { + *frame_id = ns + std::string("/") + *frame_id; + } +} + + +int main (int argc, char **argv) +{ + ros::init(argc, argv, "sensor_frame_node"); + + SensorFrameNode node(); + ros::spin(); + + return 0; +} \ No newline at end of file