From 0401650ff7ee190dd22ce95c78df22bb38339ad8 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Mon, 3 Jul 2023 22:04:08 -0700 Subject: [PATCH 01/12] ROS2 port: mrpt_msgs_bridge --- mrpt_msgs_bridge/CMakeLists.txt | 2 +- .../mrpt_msgs_bridge/{beacon.h => beacon.hpp} | 19 +++-- .../{landmark.h => landmark.hpp} | 22 +++--- .../{marker_msgs.h => marker_msgs.hpp} | 15 ++-- ...etwork_of_poses.h => network_of_poses.hpp} | 40 +++++------ mrpt_msgs_bridge/package.xml | 6 +- mrpt_msgs_bridge/src/beacon.cpp | 29 ++++---- mrpt_msgs_bridge/src/landmark.cpp | 30 ++++---- mrpt_msgs_bridge/src/marker_msgs.cpp | 22 +++--- mrpt_msgs_bridge/src/network_of_poses.cpp | 69 +++++++++---------- 10 files changed, 125 insertions(+), 129 deletions(-) rename mrpt_msgs_bridge/include/mrpt_msgs_bridge/{beacon.h => beacon.hpp} (72%) rename mrpt_msgs_bridge/include/mrpt_msgs_bridge/{landmark.h => landmark.hpp} (69%) rename mrpt_msgs_bridge/include/mrpt_msgs_bridge/{marker_msgs.h => marker_msgs.hpp} (79%) rename mrpt_msgs_bridge/include/mrpt_msgs_bridge/{network_of_poses.h => network_of_poses.hpp} (65%) diff --git a/mrpt_msgs_bridge/CMakeLists.txt b/mrpt_msgs_bridge/CMakeLists.txt index 8d71a32..d45bc45 100644 --- a/mrpt_msgs_bridge/CMakeLists.txt +++ b/mrpt_msgs_bridge/CMakeLists.txt @@ -66,7 +66,7 @@ install(TARGETS ${PROJECT_NAME} # Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include - FILES_MATCHING PATTERN "*.h" + FILES_MATCHING PATTERN "*.hpp" ) ############# diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.hpp similarity index 72% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.hpp index 485f187..670d0a6 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/beacon.hpp @@ -2,24 +2,23 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #pragma once - -#include #include #include -#include +#include +#include #include #include namespace mrpt_msgs_bridge { -/** @name ObservationRangeBeacon: ROS <-> MRPT +/** @name ObservationRangeBeacon: ROS2 <-> MRPT * @{ */ /** ROS->MRPT: Takes a mrpt_msgs::ObservationRangeBeacon and the relative pose @@ -28,26 +27,26 @@ namespace mrpt_msgs_bridge * \sa mrpt2ros */ bool fromROS( - const mrpt_msgs::ObservationRangeBeacon& _msg, + const mrpt_msgs::msg::ObservationRangeBeacon _msg, const mrpt::poses::CPose3D& _pose, mrpt::obs::CObservationBeaconRanges& _obj); -/** MRPT->ROS: Takes a CObservationBeaconRanges and outputs range data in +/** MRPT->ROS2: Takes a CObservationBeaconRanges and outputs range data in * mrpt_msgs::ObservationRangeBeacon \return true on sucessful conversion, false * on any error. \sa ros2mrpt */ bool toROS( const mrpt::obs::CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg); + mrpt_msgs::msg::ObservationRangeBeacon& _msg); -/** MRPT->ROS: Takes a CObservationBeaconRanges and outputs range data in +/** MRPT->ROS2: Takes a CObservationBeaconRanges and outputs range data in * mrpt_msgs::ObservationRangeBeacon + the relative pose of the range sensor wrt * base_link \return true on sucessful conversion, false on any error. \sa * ros2mrpt */ bool toROS( const mrpt::obs::CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg, geometry_msgs::Pose& _pose); + mrpt_msgs::msg::ObservationRangeBeacon& _msg, geometry_msgs::msg::Pose& _pose); /** @} */ diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.hpp similarity index 69% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.hpp index 1cfcadc..aad702a 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/landmark.hpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -17,35 +17,35 @@ #include #include -#include -#include +#include +#include #include namespace mrpt_msgs_bridge { -/** @name LaserScan: ROS <-> MRPT +/** @name LaserScan: ROS2 <-> MRPT * @{ */ -/** ROS->MRPT: Takes a mrpt_msgs::CObservationBearingRange and the relative pose +/** ROS2->MRPT: Takes a mrpt_msgs::CObservationBearingRange and the relative pose * of the sensor wrt base_link and builds a ObservationRangeBearing * \return true on sucessful conversion, false on any error. * \sa mrpt2ros */ bool fromROS( - const mrpt_msgs::ObservationRangeBearing& _msg, + const mrpt_msgs::msg::ObservationRangeBearing& _msg, const mrpt::poses::CPose3D& _pose, mrpt::obs::CObservationBearingRange& _obj ); -/** MRPT->ROS: Takes a CObservationBearingRange and outputs range data in +/** MRPT->ROS2: Takes a CObservationBearingRange and outputs range data in * mrpt_msgs::ObservationRangeBearing * \return true on sucessful conversion, false on any error. * \sa ros2mrpt */ bool toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg); + mrpt_msgs::msg::ObservationRangeBearing& _msg); -/** MRPT->ROS: Takes a CObservationBearingRange and outputs range data in +/** MRPT->ROS2: Takes a CObservationBearingRange and outputs range data in * mrpt_msgs::ObservationRangeBearing + the relative pose of the range sensor * wrt base_link * \return true on sucessful conversion, false on any error. @@ -53,8 +53,8 @@ bool toROS( */ bool toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg, geometry_msgs::Pose& sensorPose); + mrpt_msgs::msg::ObservationRangeBearing& _msg, geometry_msgs::msg::Pose& sensorPose); /** @} */ -} // namespace mrpt_bridge +} // namespace mrpt_bridge \ No newline at end of file diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.hpp similarity index 79% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.hpp index 65e1df3..89ded9b 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/marker_msgs.hpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | - | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -16,8 +16,9 @@ #pragma once -#include -#include +#include +#include + #include #include #include @@ -26,18 +27,18 @@ namespace mrpt_msgs_bridge { -// NOTE: These converters are here instead of mrpt::ros1bridge since +// NOTE: These converters are here instead of mrpt::ros2bridge since // the builds dep. MarkerDetection is not available in Debian/Ubuntu // official repos, so we need to build it here within ROS. bool fromROS( - const marker_msgs::MarkerDetection& _msg, + const marker_msgs::msg::MarkerDetection& _msg, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBearingRange& _obj); bool fromROS( - const marker_msgs::MarkerDetection& _msg, + const marker_msgs::msg::MarkerDetection& _msg, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBeaconRanges& _obj); -} // namespace mrpt_msgs_bridge +} // namespace mrpt_msgs_bridge \ No newline at end of file diff --git a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.h b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.hpp similarity index 65% rename from mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.h rename to mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.hpp index 1ff1c1b..3de9b82 100644 --- a/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.h +++ b/mrpt_msgs_bridge/include/mrpt_msgs_bridge/network_of_poses.hpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -14,12 +14,12 @@ #pragma once #include -#include +#include namespace mrpt_msgs_bridge { -/**\name MRPT \rightarrow ROS conversions */ -/**\brief Convert [MRPT] CNetworkOfPoses*D \rightarrow [ROS] NetworkOfPoses. +/**\name MRPT \rightarrow ROS2 conversions */ +/**\brief Convert [MRPT] CNetworkOfPoses*D \rightarrow [ROS2] NetworkOfPoses. * \param[in] mrpt_graph MRPT graph representation * \param[out] ros_graph ROS graph representation */ @@ -28,62 +28,62 @@ namespace mrpt_msgs_bridge // TODO - convert these methods into a common polymorphic method void toROS( const mrpt::graphs::CNetworkOfPoses2D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses3D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void toROS( const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); /**\} */ ///////////////////////////////////////////////////////////////////////// -/**\name ROS \rightarrow MRPT conversions */ -/**\brief Convert [ROS] NetworkOfPoses \rightarrow [MRPT] CNetworkOfPoses*DInf. - * \param[in] mrpt_graph ROS graph representation +/**\name ROS2 \rightarrow MRPT conversions */ +/**\brief Convert [ROS2] NetworkOfPoses \rightarrow [MRPT] CNetworkOfPoses*DInf. + * \param[in] mrpt_graph ROS2 graph representation * \param[out] ros_graph MRPT graph representation */ /**\{ */ void fromROS( const mrpt::graphs::CNetworkOfPoses2D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void fromROS( const mrpt::graphs::CNetworkOfPoses3D& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph); + mrpt_msgs::msg::NetworkOfPoses& ros_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph); void fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph); /**\} */ -} // namespace mrpt_msgs_bridge +} // namespace mrpt_msgs_bridge \ No newline at end of file diff --git a/mrpt_msgs_bridge/package.xml b/mrpt_msgs_bridge/package.xml index d0f0b22..77f3ce6 100644 --- a/mrpt_msgs_bridge/package.xml +++ b/mrpt_msgs_bridge/package.xml @@ -6,7 +6,11 @@ https://www.mrpt.org/ José Luis Blanco-Claraco + Shravan S Rai + José Luis Blanco-Claraco + Shravan S Rai + BSD https://wiki.ros.org/mrpt_msgs_bridge @@ -22,7 +26,7 @@ tf2 - catkin + ament diff --git a/mrpt_msgs_bridge/src/beacon.cpp b/mrpt_msgs_bridge/src/beacon.cpp index 61045b8..e5c8ae9 100644 --- a/mrpt_msgs_bridge/src/beacon.cpp +++ b/mrpt_msgs_bridge/src/beacon.cpp @@ -2,17 +2,14 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ +#include +#include +#include -#include -#include -#include -#include -#include -#include #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/transform_datatypes.h" @@ -20,10 +17,10 @@ using namespace mrpt::obs; bool mrpt_msgs_bridge::fromROS( - const mrpt_msgs::ObservationRangeBeacon& _msg, + const mrpt_msgs::msg::ObservationRangeBeacon _msg, const mrpt::poses::CPose3D& _pose, CObservationBeaconRanges& _obj) { - _obj.timestamp = mrpt::ros1bridge::fromROS(_msg.header.stamp); + _obj.timestamp = mrpt::ros2bridge::fromROS(_msg.header.stamp); mrpt::poses::CPose3D cpose_obj; _obj.stdError = _msg.sensor_std_range; @@ -33,7 +30,7 @@ bool mrpt_msgs_bridge::fromROS( if (_pose.empty()) { - cpose_obj = mrpt::ros1bridge::fromROS(_msg.sensor_pose_on_robot); + cpose_obj = mrpt::ros2bridge::fromROS(_msg.sensor_pose_on_robot); _obj.setSensorPose(cpose_obj); } else @@ -56,13 +53,13 @@ bool mrpt_msgs_bridge::fromROS( bool mrpt_msgs_bridge::toROS( const CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg) + mrpt_msgs::msg::ObservationRangeBeacon& _msg) { mrpt::poses::CPose3D cpose_obj; - _msg.header.stamp = mrpt::ros1bridge::toROS(_obj.timestamp); + _msg.header.stamp = mrpt::ros2bridge::toROS(_obj.timestamp); _obj.getSensorPose(cpose_obj); - _msg.sensor_pose_on_robot = mrpt::ros1bridge::toROS_Pose(cpose_obj); + _msg.sensor_pose_on_robot = mrpt::ros2bridge::toROS_Pose(cpose_obj); _msg.sensor_std_range = _obj.stdError; _msg.header.frame_id = _obj.sensorLabel; @@ -84,11 +81,11 @@ bool mrpt_msgs_bridge::toROS( bool mrpt_msgs_bridge::toROS( const CObservationBeaconRanges& _obj, - mrpt_msgs::ObservationRangeBeacon& _msg, geometry_msgs::Pose& _pose) + mrpt_msgs::msg::ObservationRangeBeacon& _msg, geometry_msgs::msg::Pose& _pose) { toROS(_obj, _msg); mrpt::poses::CPose3D pose; _obj.getSensorPose(pose); - _pose = mrpt::ros1bridge::toROS_Pose(pose); + _pose = mrpt::ros2bridge::toROS_Pose(pose); return true; -} +} \ No newline at end of file diff --git a/mrpt_msgs_bridge/src/landmark.cpp b/mrpt_msgs_bridge/src/landmark.cpp index 000d9ac..246c54f 100644 --- a/mrpt_msgs_bridge/src/landmark.cpp +++ b/mrpt_msgs_bridge/src/landmark.cpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -13,20 +13,18 @@ * */ -#include -#include -#include -#include -#include -#include +#include +#include +#include + bool mrpt_msgs_bridge::fromROS( - const mrpt_msgs::ObservationRangeBearing& _msg, + const mrpt_msgs::msg::ObservationRangeBearing& _msg, const mrpt::poses::CPose3D& _pose, mrpt::obs::CObservationBearingRange& _obj) { - _obj.timestamp = mrpt::ros1bridge::fromROS(_msg.header.stamp); + _obj.timestamp = mrpt::ros2bridge::fromROS(_msg.header.stamp); mrpt::poses::CPose3D cpose_obj; @@ -41,7 +39,7 @@ bool mrpt_msgs_bridge::fromROS( if (_pose.empty()) { _obj.setSensorPose( - mrpt::ros1bridge::fromROS(_msg.sensor_pose_on_robot)); + mrpt::ros2bridge::fromROS(_msg.sensor_pose_on_robot)); } else { @@ -65,11 +63,11 @@ bool mrpt_msgs_bridge::fromROS( bool mrpt_msgs_bridge::toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg) + mrpt_msgs::msg::ObservationRangeBearing& _msg) { - _msg.header.stamp = mrpt::ros1bridge::toROS(_obj.timestamp); + _msg.header.stamp = mrpt::ros2bridge::toROS(_obj.timestamp); - _msg.sensor_pose_on_robot = mrpt::ros1bridge::toROS_Pose(_obj.sensorPose()); + _msg.sensor_pose_on_robot = mrpt::ros2bridge::toROS_Pose(_obj.sensorPose()); _msg.max_sensor_distance = _obj.maxSensorDistance; _msg.min_sensor_distance = _obj.minSensorDistance; @@ -94,9 +92,9 @@ bool mrpt_msgs_bridge::toROS( bool mrpt_msgs_bridge::toROS( const mrpt::obs::CObservationBearingRange& _obj, - mrpt_msgs::ObservationRangeBearing& _msg, geometry_msgs::Pose& sensorPose) + mrpt_msgs::msg::ObservationRangeBearing& _msg, geometry_msgs::msg::Pose& sensorPose) { toROS(_obj, _msg); - sensorPose = mrpt::ros1bridge::toROS_Pose(_obj.sensorPose()); + sensorPose = mrpt::ros2bridge::toROS_Pose(_obj.sensorPose()); return true; -} +} \ No newline at end of file diff --git a/mrpt_msgs_bridge/src/marker_msgs.cpp b/mrpt_msgs_bridge/src/marker_msgs.cpp index 4dcbf43..864f973 100644 --- a/mrpt_msgs_bridge/src/marker_msgs.cpp +++ b/mrpt_msgs_bridge/src/marker_msgs.cpp @@ -2,7 +2,7 @@ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | - | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ @@ -14,17 +14,15 @@ * @brief Funtions to convert marker_msgs to mrpt msgs **/ -#include -#include -#include -#include +#include +#include bool mrpt_msgs_bridge::fromROS( - const marker_msgs::MarkerDetection& src, + const marker_msgs::msg::MarkerDetection& src, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBearingRange& des) { - des.timestamp = mrpt::ros1bridge::fromROS(src.header.stamp); + des.timestamp = mrpt::ros2bridge::fromROS(src.header.stamp); des.setSensorPose(sensorPoseOnRobot); des.minSensorDistance = src.distance_min; des.maxSensorDistance = src.distance_max; @@ -32,7 +30,7 @@ bool mrpt_msgs_bridge::fromROS( des.sensedData.resize(src.markers.size()); for (size_t i = 0; i < src.markers.size(); i++) { - const marker_msgs::Marker& marker = src.markers[i]; + const marker_msgs::msg::Marker& marker = src.markers[i]; // mrpt::obs::CObservationBearingRange::TMeasurement auto& m = des.sensedData[i]; m.range = sqrt( @@ -53,11 +51,11 @@ bool mrpt_msgs_bridge::fromROS( } bool mrpt_msgs_bridge::fromROS( - const marker_msgs::MarkerDetection& src, + const marker_msgs::msg::MarkerDetection& src, const mrpt::poses::CPose3D& sensorPoseOnRobot, mrpt::obs::CObservationBeaconRanges& des) { - des.timestamp = mrpt::ros1bridge::fromROS(src.header.stamp); + des.timestamp = mrpt::ros2bridge::fromROS(src.header.stamp); des.setSensorPose(sensorPoseOnRobot); des.minSensorDistance = src.distance_min; @@ -66,7 +64,7 @@ bool mrpt_msgs_bridge::fromROS( des.sensedData.resize(src.markers.size()); for (size_t i = 0; i < src.markers.size(); i++) { - const marker_msgs::Marker& marker = src.markers[i]; + const marker_msgs::msg::Marker& marker = src.markers[i]; // mrpt::obs::CObservationBeaconRanges::TMeasurement auto& m = des.sensedData[i]; m.sensedDistance = sqrt( @@ -83,4 +81,4 @@ bool mrpt_msgs_bridge::fromROS( } } return true; -} +} \ No newline at end of file diff --git a/mrpt_msgs_bridge/src/network_of_poses.cpp b/mrpt_msgs_bridge/src/network_of_poses.cpp index ccb359a..9f2f546 100644 --- a/mrpt_msgs_bridge/src/network_of_poses.cpp +++ b/mrpt_msgs_bridge/src/network_of_poses.cpp @@ -2,32 +2,31 @@ | Mobile Robot Programming Toolkit (MRPT) | | http://www.mrpt.org/ | | | - | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | | See: http://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See details in http://www.mrpt.org/License | +------------------------------------------------------------------------+ */ -#include -#include +#include +#include #include -#include -#include -#include +#include +#include #include // for debugging reasons using mrpt::graphs::TNodeID; /////////////////////////////////////////////////////////////////////////////////////////// -// MRPT => ROS +// MRPT => ROS2 /////////////////////////////////////////////////////////////////////////////////////////// void mrpt_msgs_bridge::toROS( const mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph) + mrpt_msgs::msg::NetworkOfPoses& ros_graph) { MRPT_START - using namespace geometry_msgs; + using namespace geometry_msgs::msg; using namespace mrpt::math; using namespace mrpt::graphs; using namespace mrpt::poses; @@ -46,12 +45,12 @@ void mrpt_msgs_bridge::toROS( for (poses_cit_t poses_cit = mrpt_graph.nodes.begin(); poses_cit != mrpt_graph.nodes.end(); ++poses_cit) { - mrpt_msgs::NodeIDWithPose ros_node; + mrpt_msgs::msg::NodeIDWithPose ros_node; // nodeID ros_node.node_id = poses_cit->first; // pose - ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second); + ros_node.pose = mrpt::ros2bridge::toROS_Pose(poses_cit->second); // zero the optional fields ros_node.str_id.data = ""; @@ -63,7 +62,7 @@ void mrpt_msgs_bridge::toROS( // fill the constraints for (const auto& edge : constraints) { - mrpt_msgs::GraphConstraint ros_constr; + mrpt_msgs::msg::GraphConstraint ros_constr; // constraint ends ros_constr.node_id_from = edge.first.first; @@ -74,11 +73,11 @@ void mrpt_msgs_bridge::toROS( { CPosePDFGaussianInf constr_inv; edge.second.inverse(constr_inv); - ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv); + ros_constr.constraint = mrpt::ros2bridge::toROS(constr_inv); } else { - ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second); + ros_constr.constraint = mrpt::ros2bridge::toROS(edge.second); } ros_graph.constraints.push_back(ros_constr); @@ -89,7 +88,7 @@ void mrpt_msgs_bridge::toROS( void mrpt_msgs_bridge::toROS( [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph, - [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph) + [[maybe_unused]] mrpt_msgs::msg::NetworkOfPoses& ros_graph) { THROW_EXCEPTION("Conversion not implemented yet"); // TODO: Implement CNetworkOfPoses3DInf => mrpt_msgs::NetworkOfPoses @@ -98,11 +97,11 @@ void mrpt_msgs_bridge::toROS( void mrpt_msgs_bridge::toROS( const mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph, - mrpt_msgs::NetworkOfPoses& ros_graph) + mrpt_msgs::msg::NetworkOfPoses& ros_graph) { MRPT_START - using namespace geometry_msgs; + using namespace geometry_msgs::msg; using namespace mrpt::math; using namespace mrpt::graphs; using namespace mrpt::poses; @@ -128,12 +127,12 @@ void mrpt_msgs_bridge::toROS( for (poses_cit_t poses_cit = mrpt_graph.nodes.begin(); poses_cit != mrpt_graph.nodes.end(); ++poses_cit) { - mrpt_msgs::NodeIDWithPose ros_node; + mrpt_msgs::msg::NodeIDWithPose ros_node; // nodeID ros_node.node_id = poses_cit->first; // pose - ros_node.pose = mrpt::ros1bridge::toROS_Pose(poses_cit->second); + ros_node.pose = mrpt::ros2bridge::toROS_Pose(poses_cit->second); // optional fields for the MR-SLAM case ros_node.str_id.data = poses_cit->second.agent_ID_str; @@ -146,7 +145,7 @@ void mrpt_msgs_bridge::toROS( // CNetworkOfPoses2DInf for (const auto& edge : constraints) { - mrpt_msgs::GraphConstraint ros_constr; + mrpt_msgs::msg::GraphConstraint ros_constr; // constraint ends ros_constr.node_id_from = edge.first.first; @@ -157,11 +156,11 @@ void mrpt_msgs_bridge::toROS( { CPosePDFGaussianInf constr_inv; edge.second.inverse(constr_inv); - ros_constr.constraint = mrpt::ros1bridge::toROS(constr_inv); + ros_constr.constraint = mrpt::ros2bridge::toROS(constr_inv); } else { - ros_constr.constraint = mrpt::ros1bridge::toROS(edge.second); + ros_constr.constraint = mrpt::ros2bridge::toROS(edge.second); } ros_graph.constraints.push_back(ros_constr); @@ -172,22 +171,22 @@ void mrpt_msgs_bridge::toROS( void mrpt_msgs_bridge::toROS( [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph, - [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph) + [[maybe_unused]] mrpt_msgs::msg::NetworkOfPoses& ros_graph) { THROW_EXCEPTION("Conversion not implemented yet"); } /////////////////////////////////////////////////////////////////////////////////////////// -// ROS => MRPT +// ROS2 => MRPT /////////////////////////////////////////////////////////////////////////////////////////// void mrpt_msgs_bridge::fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf& mrpt_graph) { MRPT_START using namespace mrpt::poses; - using namespace mrpt_msgs; + using namespace mrpt_msgs::msg; using namespace std; typedef NetworkOfPoses::_nodes_type::_vec_type::const_iterator nodes_cit_t; @@ -201,7 +200,7 @@ void mrpt_msgs_bridge::fromROS( nodes_cit != ros_graph.nodes.vec.end(); ++nodes_cit) { // get the pose - CPose2D mrpt_pose = CPose2D(mrpt::ros1bridge::fromROS(nodes_cit->pose)); + CPose2D mrpt_pose = CPose2D(mrpt::ros2bridge::fromROS(nodes_cit->pose)); mrpt_graph.nodes.insert( make_pair(static_cast(nodes_cit->node_id), mrpt_pose)); @@ -218,7 +217,7 @@ void mrpt_msgs_bridge::fromROS( // constraint value const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf( - mrpt::ros1bridge::fromROS(constr_cit->constraint)); + mrpt::ros2bridge::fromROS(constr_cit->constraint)); mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr)); } @@ -229,7 +228,7 @@ void mrpt_msgs_bridge::fromROS( } void mrpt_msgs_bridge::fromROS( - [[maybe_unused]] const mrpt_msgs::NetworkOfPoses& ros_graph, + [[maybe_unused]] const mrpt_msgs::msg::NetworkOfPoses& ros_graph, [[maybe_unused]] mrpt::graphs::CNetworkOfPoses3DInf& mrpt_graph) { THROW_EXCEPTION("Conversion not implemented yet"); @@ -238,12 +237,12 @@ void mrpt_msgs_bridge::fromROS( } void mrpt_msgs_bridge::fromROS( - const mrpt_msgs::NetworkOfPoses& ros_graph, + const mrpt_msgs::msg::NetworkOfPoses& ros_graph, mrpt::graphs::CNetworkOfPoses2DInf_NA& mrpt_graph) { MRPT_START using namespace mrpt::poses; - using namespace mrpt_msgs; + using namespace mrpt_msgs::msg; using namespace std; using nodes_cit_t = NetworkOfPoses::_nodes_type::_vec_type::const_iterator; @@ -261,7 +260,7 @@ void mrpt_msgs_bridge::fromROS( { // set the nodeID/pose mrpt_graph_pose_t mrpt_node = - mrpt::ros1bridge::fromROS(nodes_cit->pose); + mrpt::ros2bridge::fromROS(nodes_cit->pose); // set the MR-SLAM fields mrpt_node.agent_ID_str = nodes_cit->str_id.data; @@ -282,7 +281,7 @@ void mrpt_msgs_bridge::fromROS( // constraint value const auto mrpt_constr = mrpt::poses::CPosePDFGaussianInf( - mrpt::ros1bridge::fromROS(constr_cit->constraint)); + mrpt::ros2bridge::fromROS(constr_cit->constraint)); mrpt_graph.edges.insert(make_pair(constr_ends, mrpt_constr)); } @@ -293,8 +292,8 @@ void mrpt_msgs_bridge::fromROS( } void convert( - [[maybe_unused]] mrpt_msgs::NetworkOfPoses& ros_graph, + [[maybe_unused]] mrpt_msgs::msg::NetworkOfPoses& ros_graph, [[maybe_unused]] const mrpt::graphs::CNetworkOfPoses3DInf_NA& mrpt_graph) { THROW_EXCEPTION("Conversion not implemented yet"); -} +} \ No newline at end of file From c8e2dce79a69e0f3d8125c1e186f27683ba7a6ab Mon Sep 17 00:00:00 2001 From: SRai22 Date: Mon, 3 Jul 2023 22:13:23 -0700 Subject: [PATCH 02/12] ROS2 port metapackage --- mrpt_navigation/CMakeLists.txt | 12 +++++++++--- mrpt_navigation/package.xml | 4 ++-- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/mrpt_navigation/CMakeLists.txt b/mrpt_navigation/CMakeLists.txt index eb6284f..aa2c0b2 100644 --- a/mrpt_navigation/CMakeLists.txt +++ b/mrpt_navigation/CMakeLists.txt @@ -1,4 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) + project(mrpt_navigation) -find_package(catkin REQUIRED) -catkin_metapackage() + +# Find dependencies +find_package(ament_cmake REQUIRED) + + +# The following is needed for ROS 2's ament to generate the setup hooks +ament_package() \ No newline at end of file diff --git a/mrpt_navigation/package.xml b/mrpt_navigation/package.xml index d19561f..621deb7 100644 --- a/mrpt_navigation/package.xml +++ b/mrpt_navigation/package.xml @@ -13,7 +13,7 @@ BSD https://wiki.ros.org/mrpt_navigation - catkin + ament_cmake mrpt_local_obstacles mrpt_localization @@ -23,6 +23,6 @@ mrpt_tutorials - + ament_cmake From e9f4bc752d55595cf55d69e70e266ce60a3ff5b2 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Tue, 4 Jul 2023 15:38:18 -0700 Subject: [PATCH 03/12] progress --- .gitmodules | 6 + mrpt_local_obstacles/CMakeLists.txt | 123 +++-- .../mrpt_local_obstacles_node.hpp | 149 ++++++ mrpt_local_obstacles/package.xml | 14 +- .../src/mrpt_local_obstacles_node.cpp | 478 ++++++------------ mrpt_local_obstacles/submodules/mola-common | 1 + mrpt_local_obstacles/submodules/mp2p_icp | 1 + 7 files changed, 383 insertions(+), 389 deletions(-) create mode 100644 .gitmodules create mode 100644 mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp create mode 160000 mrpt_local_obstacles/submodules/mola-common create mode 160000 mrpt_local_obstacles/submodules/mp2p_icp diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..3ad9c9e --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "mrpt_local_obstacles/submodules/mola-common"] + path = mrpt_local_obstacles/submodules/mola-common + url = https://github.com/MOLAorg/mola-common.git +[submodule "mrpt_local_obstacles/submodules/mp2p_icp"] + path = mrpt_local_obstacles/submodules/mp2p_icp + url = https://github.com/MOLAorg/mp2p_icp.git diff --git a/mrpt_local_obstacles/CMakeLists.txt b/mrpt_local_obstacles/CMakeLists.txt index a020c9e..71a5e42 100644 --- a/mrpt_local_obstacles/CMakeLists.txt +++ b/mrpt_local_obstacles/CMakeLists.txt @@ -1,29 +1,35 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.5) project(mrpt_local_obstacles) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure - roscpp - sensor_msgs - tf2 - visualization_msgs - tf2_geometry_msgs -) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) ## System dependencies are found with CMake's conventions find_package(mrpt-maps REQUIRED) find_package(mrpt-obs REQUIRED) find_package(mrpt-gui REQUIRED) -find_package(mrpt-ros1bridge REQUIRED) +find_package(mrpt-ros2bridge REQUIRED) + +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() if (CMAKE_COMPILER_IS_GNUCXX) # High level of warnings. # The -Wno-long-long is required in 64bit systems when including sytem headers. # The -Wno-variadic-macros was needed for Eigen3, StdVector.h - add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros) + add_compile_options(-Wall -Wno-long-long -Wno-variadic-macros -Wextra -Wpedantic) # Workaround: Eigen <3.4 produces *tons* of warnings in GCC >=6. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1221 if (NOT ${CMAKE_CXX_COMPILER_VERSION} LESS "6.0") add_compile_options(-Wno-ignored-attributes -Wno-int-in-bool-context) @@ -33,59 +39,64 @@ IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") add_compile_options(-O3) ENDIF() -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs tf2 tf2_geometry_msgs visualization_msgs - # DEPENDS mrpt -) +# TODO: Remove and move as an independent ROS package: +add_subdirectory(submodules/mola-common) +set(mola-common_DIR ${CMAKE_BINARY_DIR} CACHE PATH "path to mola-common-config.cmake" FORCE) -########### -## Build ## -########### +add_subdirectory(submodules/mp2p_icp) -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - ${catkin_INCLUDE_DIRS} -) +add_executable(${PROJECT_NAME} + src/mrpt_local_obstacles_node.cpp + include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp) -## Declare a cpp executable -add_executable(${PROJECT_NAME}_node - src/mrpt_local_obstacles_node.cpp +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ ) -# Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node - ${catkin_LIBRARIES} +target_link_libraries( + ${PROJECT_NAME} mrpt::maps mrpt::obs mrpt::gui - mrpt::ros1bridge + mrpt::ros2bridge + mp2p_icp_filters +) + +ament_target_dependencies( + ${PROJECT_NAME} + "rclcpp" + "rclcpp_components" + "nav_msgs" + "sensor_msgs" + "tf2" + "tf2_geometry_msgs" + "visualization_msgs" ) -############# -## Install ## -############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} +) -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() + diff --git a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp new file mode 100644 index 0000000..54ab13f --- /dev/null +++ b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp @@ -0,0 +1,149 @@ +/*********************************************************************************** + * Revised BSD License * + * Copyright (c) 2014-2023, Jose-Luis Blanco * + * All rights reserved. * + * * + * 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 Vienna University of Technology 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 Markus Bader 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. * + ***********************************************************************************/ + +/* mrpt deps*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* ros2 deps */ +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace mrpt::system; +using namespace mrpt::config; +using namespace mrpt::img; +using namespace mrpt::maps; +using namespace mrpt::obs; + +class LocalObstaclesNode: public rclcpp::Node +{ + public: + /* Ctor*/ + explicit LocalObstaclesNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + /* Dtor*/ + ~LocalObstaclesNode(){} + + private: + /* Read parameters from the node handle*/ + void read_parameters(); + + /* Callback: On recalc local map & publish it*/ + void on_do_publish(); + + /* Callback: On new sensor data*/ + void on_new_sensor_laser_2d(const sensor_msgs::msg::LaserScan::SharedPtr& scan); + + /* Callback: On new pointcloud data*/ + void on_new_sensor_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr& pts); + + /** + * @brief Subscribe to a variable number of topics. + * @param lstTopics String with list of topics separated with "," + * @param subs[in,out] List of subscribers will be here at return. + * @return The number of topics subscribed to. + */ + template + size_t subscribe_to_multiple_topics( + const std::string& lstTopics, + std::vector::SharedPtr>& subscriptions, + CallbackMethodType callback); + + // member variables + CTimeLogger m_profiler; + bool m_show_gui = false; + std::string m_frameid_reference = "ddom"; //!< type:"odom" + std::string m_frameid_robot = "base_link"; //!< type: "base_link" + std::string m_topic_local_map_pointcloud = "local_map_pointcloud"; + //!< Default: "local_map_pointcloud" + std::string m_topics_source_2dscan = "scan, laser1"; //!< Default: "scan, laser1" + std::string m_topics_source_pointclouds = ""; + + double m_time_window = 0.20; //!< In secs (default: 0.2). Can't be smaller than m_publish_period + double m_publish_period = 0.05; //!< In secs (default: 0.05). Can't be larger than m_time_window + + rclcpp::TimerBase::SharedPtr m_timer_publish; + + // Sensor data: + struct TInfoPerTimeStep + { + CObservation::Ptr observation; + mrpt::poses::CPose3D robot_pose; + }; + typedef std::multimap TListObservations; + + TListObservations m_hist_obs; //!< The history of past observations during + //! the interest time window. + + std::mutex m_hist_obs_mtx; //!< mutex + + CSimplePointsMap::Ptr m_localmap_pts = CSimplePointsMap::Create(); + + mrpt::gui::CDisplayWindow3D::Ptr m_gui_win; + + /// Used for example to run voxel grid decimation, etc. + /// Refer to mp2p_icp docs + mp2p_icp_filters::FilterPipeline m_filter_pipeline; + + std::string m_filter_output_layer_name; //!< mp2p_icp output layer name + + /** + * @name ROS2 pubs/subs + * @{ + */ + rclcpp::Publisher::SharedPtr m_pub_local_map_pointcloud; + std::vector::SharedPtr> m_subs_2dlaser; + std::vector::SharedPtr> m_subs_pointclouds; + + std::shared_ptr m_tf_buffer; + std::shared_ptr m_tf_listener; + +}; \ No newline at end of file diff --git a/mrpt_local_obstacles/package.xml b/mrpt_local_obstacles/package.xml index f9fe097..604d367 100644 --- a/mrpt_local_obstacles/package.xml +++ b/mrpt_local_obstacles/package.xml @@ -1,5 +1,5 @@ - + mrpt_local_obstacles 1.0.3 Maintains a local obstacle map (point cloud, @@ -7,23 +7,29 @@ configurable time window. Jose-Luis Blanco-Claraco + Shravan S Rai + + Jose-Luis Blanco-Claraco + Shravan S Rai BSD https://wiki.ros.org/mrpt_local_obstacles - Jose-Luis Blanco-Claraco - catkin + ament_cmake mrpt2 dynamic_reconfigure - roscpp + rclcpp + rclcpp_components + nav_msgs sensor_msgs tf2 tf2_geometry_msgs visualization_msgs + ament_cmake diff --git a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp index 03899f9..9f92582 100644 --- a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp +++ b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp @@ -1,57 +1,5 @@ -/*********************************************************************************** - * Revised BSD License * - * Copyright (c) 2014-2015, Jose-Luis Blanco * - * All rights reserved. * - * * - * 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 Vienna University of Technology 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 Markus Bader 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include "rclcpp_components/register_node_macro.hpp" using namespace mrpt::system; using namespace mrpt::config; @@ -59,228 +7,101 @@ using namespace mrpt::img; using namespace mrpt::maps; using namespace mrpt::obs; -// The ROS node -class LocalObstaclesNode -{ - private: - struct TAuxInitializer - { - TAuxInitializer(int argc, char** argv) - { - ros::init(argc, argv, "mrpt_local_obstacles_node"); - } - }; - - CTimeLogger m_profiler; - - TAuxInitializer m_auxinit; //!< Just to make sure ROS is init first - ros::NodeHandle m_nh; //!< The node handle - ros::NodeHandle m_localn; //!< "~" - bool m_show_gui; - std::string m_frameid_reference; //!< typ: "odom" - std::string m_frameid_robot; //!< typ: "base_link" - std::string - m_topic_local_map_pointcloud; //!< Default: "local_map_pointcloud" - std::string m_source_topics_2dscan; //!< Default: "scan,laser1" - double m_time_window; //!< In secs (default: 0.2). This can't be smaller - //! than m_publish_period - double m_publish_period; //!< In secs (default: 0.05). This can't be larger - //! than m_time_window - - ros::Timer m_timer_publish; - - // Sensor data: - struct TInfoPerTimeStep - { - CObservation::Ptr observation; - mrpt::poses::CPose3D robot_pose; - }; - typedef std::multimap TListObservations; - TListObservations m_hist_obs; //!< The history of past observations during - //! the interest time window. - boost::mutex m_hist_obs_mtx; - - /** The local maps */ - CSimplePointsMap m_localmap_pts; - // COccupancyGridMap2D m_localmap_grid; - - mrpt::gui::CDisplayWindow3D::Ptr m_gui_win; - - /** @name ROS pubs/subs - * @{ */ - ros::Publisher m_pub_local_map_pointcloud; - - //!< Subscriber to 2D laser scans - std::vector m_subs_2dlaser; - - tf2_ros::Buffer m_tf_buffer; - tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; - - /** @} */ - - /** - * @brief Subscribe to a variable number of topics. - * @param lstTopics String with list of topics separated with "," - * @param subs[in,out] List of subscribers will be here at return. - * @return The number of topics subscribed to. - */ - template - size_t subscribeToMultipleTopics( - const std::string& lstTopics, std::vector& subs, - CALLBACK_METHOD_TYPE cb) - { - std::vector lstSources; - mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources); - subs.resize(lstSources.size()); - for (size_t i = 0; i < lstSources.size(); i++) - subs[i] = m_nh.subscribe(lstSources[i], 1, cb, this); - return lstSources.size(); - } - - /** Callback: On new sensor data - */ - void onNewSensor_Laser2D(const sensor_msgs::LaserScanConstPtr& scan) - { - CTimeLoggerEntry tle(m_profiler, "onNewSensor_Laser2D"); - - ros::Duration timeout(1.0); - - // Get the relative position of the sensor wrt the robot: - geometry_msgs::TransformStamped sensorOnRobot; - try - { - CTimeLoggerEntry tle2( - m_profiler, "onNewSensor_Laser2D.lookupTransform_sensor"); - - sensorOnRobot = m_tf_buffer.lookupTransform( - m_frameid_robot, scan->header.frame_id, scan->header.stamp, - timeout); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } - // Convert data to MRPT format: - const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() { - tf2::Transform tx; - tf2::fromMsg(sensorOnRobot.transform, tx); - return mrpt::ros1bridge::fromROS(tx); - }(); - - // In MRPT, CObservation2DRangeScan holds both: sensor data + - // relative pose: - auto obsScan = CObservation2DRangeScan::Create(); - mrpt::ros1bridge::fromROS(*scan, sensorOnRobot_mrpt, *obsScan); - - ROS_DEBUG( - "[onNewSensor_Laser2D] %u rays, sensor pose on robot %s", - static_cast(obsScan->getScanSize()), - sensorOnRobot_mrpt.asString().c_str()); - - // Get sensor timestamp: - const double timestamp = scan->header.stamp.toSec(); - - // Get robot pose at that time in the reference frame, typ: /odom -> - // /base_link - mrpt::poses::CPose3D robotPose; - try - { - CTimeLoggerEntry tle3( - m_profiler, "onNewSensor_Laser2D.lookupTransform_robot"); +LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) +: Node("mrpt_local_obstacles", options) +{ + read_parameters(); - geometry_msgs::TransformStamped robotTfStamp; - try - { - robotTfStamp = m_tf_buffer.lookupTransform( - m_frameid_reference, m_frameid_robot, scan->header.stamp, - timeout); - } - catch (const tf2::ExtrapolationException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } + // Create publisher for local map point cloud + m_pub_local_map_pointcloud = + create_publisher(m_topic_local_map_pointcloud, 10); - robotPose = [&]() { - tf2::Transform tx; - tf2::fromMsg(robotTfStamp.transform, tx); - return mrpt::ros1bridge::fromROS(tx); - }(); - ROS_DEBUG( - "[onNewSensor_Laser2D] robot pose %s", - robotPose.asString().c_str()); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } + // Init ROS subs: + // Subscribe to one or more laser sources: + size_t nSubsTotal = 0; + nSubsTotal += subscribe_to_multiple_topics( + m_topics_source_2dscan, m_subs_2dlaser, + std::bind(&LocalObstaclesNode::on_new_sensor_laser_2d, this, std::placeholders::_1)); - // Insert into the observation history: - TInfoPerTimeStep ipt; - ipt.observation = obsScan; - ipt.robot_pose = robotPose; + nSubsTotal += subscribe_to_multiple_topics( + m_topics_source_pointclouds, m_subs_pointclouds, + std::bind(&LocalObstaclesNode::on_new_sensor_pointcloud, this, std::placeholders::_1)); - m_hist_obs_mtx.lock(); - m_hist_obs.insert( - m_hist_obs.end(), TListObservations::value_type(timestamp, ipt)); - m_hist_obs_mtx.unlock(); + RCLCPP_INFO( this->get_logger(), + "Total number of sensor subscriptions: %u", + static_cast(nSubsTotal)); - } // end onNewSensor_Laser2D + if(!(nSubsTotal>0)) + RCLCPP_ERROR( this->get_logger(), + "*Error* It is mandatory to set at least one source topic for sensory information!"); - /** Callback: On recalc local map & publish it */ - void onDoPublish(const ros::TimerEvent&) - { - CTimeLoggerEntry tle(m_profiler, "onDoPublish"); + // Local map params: + m_localmap_pts->insertionOptions.minDistBetweenLaserPoints = 0; + m_localmap_pts->insertionOptions.also_interpolate = false; - // Purge old observations & latch a local copy: - TListObservations obs; - { - CTimeLoggerEntry tle(m_profiler, "onDoPublish.removingOld"); - m_hist_obs_mtx.lock(); + // Create the tf2 buffer and listener + m_tf_buffer = std::make_shared(this->get_clock()); + m_tf_listener = std::make_shared(*m_tf_buffer); - // Purge old obs: - if (!m_hist_obs.empty()) - { - const double last_time = m_hist_obs.rbegin()->first; - TListObservations::iterator it_first_valid = - m_hist_obs.lower_bound(last_time - m_time_window); - const size_t nToRemove = - std::distance(m_hist_obs.begin(), it_first_valid); - ROS_DEBUG( - "[onDoPublish] Removing %u old entries, last_time=%lf", - static_cast(nToRemove), last_time); - m_hist_obs.erase(m_hist_obs.begin(), it_first_valid); - } - // Local copy in this thread: - obs = m_hist_obs; - m_hist_obs_mtx.unlock(); - } + m_timer_publish = create_wall_timer( + std::chrono::duration(m_publish_period), + std::bind(&LocalObstaclesNode::on_do_publish, this)); +} //end ctor - ROS_DEBUG( - "Building local map with %u observations.", - static_cast(obs.size())); - if (obs.empty()) return; +/** Callback: On recalc local map & publish it */ +void LocalObstaclesNode::on_do_publish() +{ + CTimeLoggerEntry tle(m_profiler, "on_do_publish"); + + // Purge old observations & latch a local copy: + TListObservations obs; + { + CTimeLoggerEntry tle(m_profiler, "onDoPublish.removingOld"); + m_hist_obs_mtx.lock(); + + // Purge old obs: + if (!m_hist_obs.empty()) + { + const double last_time = m_hist_obs.rbegin()->first; + TListObservations::iterator it_first_valid = + m_hist_obs.lower_bound(last_time - m_time_window); + const size_t nToRemove = + std::distance(m_hist_obs.begin(), it_first_valid); + ROS_DEBUG( + "[onDoPublish] Removing %u old entries, last_time=%lf", + static_cast(nToRemove), last_time); + m_hist_obs.erase(m_hist_obs.begin(), it_first_valid); + } + // Local copy in this thread: + obs = m_hist_obs; + m_hist_obs_mtx.unlock(); + } + + RCLCPP_DEBUG(this->logger(), + "Building local map with %u observations.", + static_cast(obs.size())); + + if (obs.empty()) return; // Build local map(s): // ----------------------------------------------- - m_localmap_pts.clear(); + m_localmap_pts->clear(); mrpt::poses::CPose3D curRobotPose; { - CTimeLoggerEntry tle2(m_profiler, "onDoPublish.buildLocalMap"); + CTimeLoggerEntry tle2(m_profiler, "on_do_publish.buildLocalMap"); // Get the latest robot pose in the reference frame (typ: /odom -> // /base_link) // so we can build the local map RELATIVE to it: - ros::Duration timeout(1.0); + std::chrono::duration timeout(1.0); + rclcpp::Duration ros2_timeout(timeout.count()); try { geometry_msgs::TransformStamped tx; - tx = m_tf_buffer.lookupTransform( + tx = m_tf_buffer->lookupTransform( m_frameid_reference, m_frameid_robot, ros::Time(0), timeout); @@ -311,20 +132,40 @@ class LocalObstaclesNode relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); // Insert obs: - m_localmap_pts.insertObservationPtr(ipt.observation, relPose); + m_localmap_pts->insertObservationPtr(ipt.observation, relPose); } // end for } + // Filtering: + mrpt::maps::CPointsMap::Ptr filteredPts; + + if (!m_filter_pipeline.empty()) + { + mp2p_icp::metric_map_t mm; + mm.layers[mp2p_icp::metric_map_t::PT_LAYER_RAW] = m_localmap_pts; + mp2p_icp_filters::apply_filter_pipeline(m_filter_pipeline, mm); + + filteredPts = mm.point_layer(m_filter_output_layer_name); + } + else + { + filteredPts = m_localmap_pts; + } + // Publish them: if (m_pub_local_map_pointcloud.getNumSubscribers() > 0) { - sensor_msgs::PointCloudPtr msg_pts = - sensor_msgs::PointCloudPtr(new sensor_msgs::PointCloud); - msg_pts->header.frame_id = m_frameid_robot; - msg_pts->header.stamp = ros::Time(obs.rbegin()->first); + sensor_msgs::PointCloud2 msg_pts; + msg_pts.header.frame_id = m_frameid_robot; + msg_pts.header.stamp = ros::Time(obs.rbegin()->first); + + auto simplPts = + std::dynamic_pointer_cast( + filteredPts); + ASSERT_(simplPts); - mrpt::ros1bridge::toROS(m_localmap_pts, msg_pts->header, *msg_pts); + mrpt::ros1bridge::toROS(*simplPts, msg_pts.header, msg_pts); m_pub_local_map_pointcloud.publish(msg_pts); } @@ -345,9 +186,15 @@ class LocalObstaclesNode gl_obs->setName("obstacles"); scene->insert(gl_obs); + auto gl_rawpts = mrpt::opengl::CPointCloud::Create(); + gl_rawpts->setName("raw_points"); + gl_rawpts->setPointSize(1.0); + gl_rawpts->setColor_u8(TColor(0x00ff00)); + scene->insert(gl_rawpts); + auto gl_pts = mrpt::opengl::CPointCloud::Create(); - gl_pts->setName("points"); - gl_pts->setPointSize(2.0); + gl_pts->setName("final_points"); + gl_pts->setPointSize(3.0); gl_pts->setColor_u8(TColor(0x0000ff)); scene->insert(gl_pts); @@ -360,8 +207,11 @@ class LocalObstaclesNode ROS_ASSERT(!!gl_obs); gl_obs->clear(); - auto gl_pts = mrpt::ptr_cast::from( - scene->getByName("points")); + auto glRawPts = mrpt::ptr_cast::from( + scene->getByName("raw_points")); + + auto glFinalPts = mrpt::ptr_cast::from( + scene->getByName("final_points")); for (const auto& o : obs) { @@ -376,7 +226,8 @@ class LocalObstaclesNode gl_obs->insert(gl_axis); } // end for - gl_pts->loadFromPointsMap(&m_localmap_pts); + glRawPts->loadFromPointsMap(m_localmap_pts.get()); + glFinalPts->loadFromPointsMap(filteredPts.get()); m_gui_win->unlockAccess3DScene(); m_gui_win->repaint(); @@ -384,71 +235,40 @@ class LocalObstaclesNode } // onDoPublish - public: - /** Constructor: Inits ROS system */ - LocalObstaclesNode(int argc, char** argv) - : m_auxinit(argc, argv), - m_nh(), - m_localn("~"), - m_show_gui(true), - m_frameid_reference("odom"), - m_frameid_robot("base_link"), - m_topic_local_map_pointcloud("local_map_pointcloud"), - m_source_topics_2dscan("scan,laser1"), - m_time_window(0.2), - m_publish_period(0.05) - { - // Load params: - m_localn.param("show_gui", m_show_gui, m_show_gui); - m_localn.param( - "frameid_reference", m_frameid_reference, m_frameid_reference); - m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot); - m_localn.param( - "topic_local_map_pointcloud", m_topic_local_map_pointcloud, - m_topic_local_map_pointcloud); - m_localn.param( - "source_topics_2dscan", m_source_topics_2dscan, - m_source_topics_2dscan); - m_localn.param("time_window", m_time_window, m_time_window); - m_localn.param("publish_period", m_publish_period, m_publish_period); - - ROS_ASSERT(m_time_window > m_publish_period); - ROS_ASSERT(m_publish_period > 0); - - // Init ROS publishers: - m_pub_local_map_pointcloud = m_nh.advertise( - m_topic_local_map_pointcloud, 10); - - // Init ROS subs: - // Subscribe to one or more laser sources: - size_t nSubsTotal = 0; - nSubsTotal += this->subscribeToMultipleTopics( - m_source_topics_2dscan, m_subs_2dlaser, - &LocalObstaclesNode::onNewSensor_Laser2D); - - ROS_INFO( - "Total number of sensor subscriptions: %u\n", - static_cast(nSubsTotal)); - ROS_ASSERT_MSG( - nSubsTotal > 0, - "*Error* It is mandatory to set at least one source topic for " - "sensory information!"); - - // Local map params: - m_localmap_pts.insertionOptions.minDistBetweenLaserPoints = 0; - m_localmap_pts.insertionOptions.also_interpolate = false; - - // Init timers: - m_timer_publish = m_nh.createTimer( - ros::Duration(m_publish_period), &LocalObstaclesNode::onDoPublish, - this); - - } // end ctor -}; // end class - -int main(int argc, char** argv) +} + +void LocalObstaclesNode::on_new_sensor_laser_2d(const sensor_msgs::msg::LaserScan::SharedPtr& scan) { - LocalObstaclesNode the_node(argc, argv); - ros::spin(); - return 0; + +} + +void LocalObstaclesNode::on_new_sensor_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr& pts) +{ + +} + +template +size_t LocalObstaclesNode::subscribe_to_multiple_topics(const std::string& lstTopics, + std::vector::SharedPtr>& subscriptions, + CallbackMethodType callback) +{ + +} + +void LocalObstaclesNode::read_parameters() +{ + +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; } diff --git a/mrpt_local_obstacles/submodules/mola-common b/mrpt_local_obstacles/submodules/mola-common new file mode 160000 index 0000000..5b2001f --- /dev/null +++ b/mrpt_local_obstacles/submodules/mola-common @@ -0,0 +1 @@ +Subproject commit 5b2001fb9b0503a3e38508cf0c5839c17047dfdb diff --git a/mrpt_local_obstacles/submodules/mp2p_icp b/mrpt_local_obstacles/submodules/mp2p_icp new file mode 160000 index 0000000..db2ac51 --- /dev/null +++ b/mrpt_local_obstacles/submodules/mp2p_icp @@ -0,0 +1 @@ +Subproject commit db2ac5127b8baee2a0e4b53bda1b2e8a5cdeb036 From c84dabfd47e37d0b3b9e64155e275ef30a5a0505 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Wed, 5 Jul 2023 23:32:48 -0700 Subject: [PATCH 04/12] progress --- mrpt_local_obstacles/CATKIN_IGNORE | 0 mrpt_local_obstacles/COLCON_IGNORE | 0 .../mrpt_local_obstacles_node.hpp | 45 +- .../src/mrpt_local_obstacles_node.cpp | 566 ++++++++++++------ 4 files changed, 423 insertions(+), 188 deletions(-) delete mode 100644 mrpt_local_obstacles/CATKIN_IGNORE delete mode 100644 mrpt_local_obstacles/COLCON_IGNORE diff --git a/mrpt_local_obstacles/CATKIN_IGNORE b/mrpt_local_obstacles/CATKIN_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mrpt_local_obstacles/COLCON_IGNORE b/mrpt_local_obstacles/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp index 54ab13f..d5fa031 100644 --- a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp +++ b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp @@ -36,12 +36,14 @@ #include #include #include +#include #include #include +#include #include #include #include -#include +#include #include #include #include @@ -91,19 +93,38 @@ class LocalObstaclesNode: public rclcpp::Node * @param subs[in,out] List of subscribers will be here at return. * @return The number of topics subscribed to. */ - template - size_t subscribe_to_multiple_topics( - const std::string& lstTopics, - std::vector::SharedPtr>& subscriptions, - CallbackMethodType callback); + template + size_t subscribe_to_multiple_topics(const std::string& lstTopics, + std::vector::SharedPtr>& subscriptions, + CallbackMethodType callback) + { + size_t num_subscriptions = 0; + // std::vector lstSources; + // mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources); + + // // Error handling: check if lstSources is empty + // if (lstSources.empty()) + // { + // RCLCPP_ERROR(this->get_logger(), "List of topics is empty."); + // return 0; // Return early with 0 subscriptions + // } + // for (const auto& source : lstSources) + // { + // const auto sub = this->create_subscription(source, 1, callback) + // subscriptions.push_back(sub); // 1 is the queue size + // num_subscriptions++; + // } + + // // Return the number of subscriptions created + return num_subscriptions; + } // member variables CTimeLogger m_profiler; bool m_show_gui = false; - std::string m_frameid_reference = "ddom"; //!< type:"odom" + std::string m_frameid_reference = "odom"; //!< type:"odom" std::string m_frameid_robot = "base_link"; //!< type: "base_link" - std::string m_topic_local_map_pointcloud = "local_map_pointcloud"; - //!< Default: "local_map_pointcloud" + std::string m_topic_local_map_pointcloud = "local_map_pointcloud"; //!< Default: "local_map_pointcloud" std::string m_topics_source_2dscan = "scan, laser1"; //!< Default: "scan, laser1" std::string m_topics_source_pointclouds = ""; @@ -132,8 +153,8 @@ class LocalObstaclesNode: public rclcpp::Node /// Used for example to run voxel grid decimation, etc. /// Refer to mp2p_icp docs mp2p_icp_filters::FilterPipeline m_filter_pipeline; - std::string m_filter_output_layer_name; //!< mp2p_icp output layer name + std::string m_filter_yaml_file; /** * @name ROS2 pubs/subs @@ -146,4 +167,6 @@ class LocalObstaclesNode: public rclcpp::Node std::shared_ptr m_tf_buffer; std::shared_ptr m_tf_listener; -}; \ No newline at end of file +}; + + diff --git a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp index 9f92582..b25c558 100644 --- a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp +++ b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp @@ -15,7 +15,7 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) // Create publisher for local map point cloud m_pub_local_map_pointcloud = - create_publisher(m_topic_local_map_pointcloud, 10); + this->create_publisher(m_topic_local_map_pointcloud, 10); // Init ROS subs: @@ -29,12 +29,12 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) m_topics_source_pointclouds, m_subs_pointclouds, std::bind(&LocalObstaclesNode::on_new_sensor_pointcloud, this, std::placeholders::_1)); - RCLCPP_INFO( this->get_logger(), + RCLCPP_INFO(this->get_logger(), "Total number of sensor subscriptions: %u", static_cast(nSubsTotal)); if(!(nSubsTotal>0)) - RCLCPP_ERROR( this->get_logger(), + RCLCPP_ERROR(this->get_logger(), "*Error* It is mandatory to set at least one source topic for sensory information!"); // Local map params: @@ -50,6 +50,7 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) std::bind(&LocalObstaclesNode::on_do_publish, this)); } //end ctor + /** Callback: On recalc local map & publish it */ void LocalObstaclesNode::on_do_publish() { @@ -57,207 +58,418 @@ void LocalObstaclesNode::on_do_publish() // Purge old observations & latch a local copy: TListObservations obs; - { - CTimeLoggerEntry tle(m_profiler, "onDoPublish.removingOld"); - m_hist_obs_mtx.lock(); - - // Purge old obs: - if (!m_hist_obs.empty()) - { - const double last_time = m_hist_obs.rbegin()->first; - TListObservations::iterator it_first_valid = - m_hist_obs.lower_bound(last_time - m_time_window); - const size_t nToRemove = - std::distance(m_hist_obs.begin(), it_first_valid); - ROS_DEBUG( - "[onDoPublish] Removing %u old entries, last_time=%lf", - static_cast(nToRemove), last_time); - m_hist_obs.erase(m_hist_obs.begin(), it_first_valid); - } - // Local copy in this thread: - obs = m_hist_obs; - m_hist_obs_mtx.unlock(); - } - - RCLCPP_DEBUG(this->logger(), - "Building local map with %u observations.", - static_cast(obs.size())); - - if (obs.empty()) return; + { + CTimeLoggerEntry tle(m_profiler, "onDoPublish.removingOld"); + m_hist_obs_mtx.lock(); - // Build local map(s): - // ----------------------------------------------- - m_localmap_pts->clear(); - mrpt::poses::CPose3D curRobotPose; + // Purge old obs: + if (!m_hist_obs.empty()) { - CTimeLoggerEntry tle2(m_profiler, "on_do_publish.buildLocalMap"); - - // Get the latest robot pose in the reference frame (typ: /odom -> - // /base_link) - // so we can build the local map RELATIVE to it: - std::chrono::duration timeout(1.0); - rclcpp::Duration ros2_timeout(timeout.count()); - - try - { - geometry_msgs::TransformStamped tx; - tx = m_tf_buffer->lookupTransform( - m_frameid_reference, m_frameid_robot, ros::Time(0), - timeout); - - tf2::Transform tfx; - tf2::fromMsg(tx.transform, tfx); - curRobotPose = mrpt::ros1bridge::fromROS(tfx); - } - catch (const tf2::ExtrapolationException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } - - ROS_DEBUG( - "[onDoPublish] Building local map relative to latest robot " - "pose: %s", - curRobotPose.asString().c_str()); - - // For each observation: compute relative robot pose & insert obs - // into map: - for (TListObservations::const_iterator it = obs.begin(); - it != obs.end(); ++it) - { - const TInfoPerTimeStep& ipt = it->second; - - // Relative pose in the past: - mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); - relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); - - // Insert obs: - m_localmap_pts->insertObservationPtr(ipt.observation, relPose); - - } // end for + const double last_time = m_hist_obs.rbegin()->first; + TListObservations::iterator it_first_valid = + m_hist_obs.lower_bound(last_time - m_time_window); + const size_t nToRemove = + std::distance(m_hist_obs.begin(), it_first_valid); + RCLCPP_DEBUG(this->get_logger(), + "[onDoPublish] Removing %u old entries, last_time=%lf", + static_cast(nToRemove), last_time); + m_hist_obs.erase(m_hist_obs.begin(), it_first_valid); } - - // Filtering: - mrpt::maps::CPointsMap::Ptr filteredPts; - - if (!m_filter_pipeline.empty()) + // Local copy in this thread: + obs = m_hist_obs; + m_hist_obs_mtx.unlock(); + } + + RCLCPP_DEBUG(this->get_logger(), + "Building local map with %u observations.", + static_cast(obs.size())); + + if (obs.empty()) return; + + // Build local map(s): + // ----------------------------------------------- + m_localmap_pts->clear(); + mrpt::poses::CPose3D curRobotPose; + { + CTimeLoggerEntry tle2(m_profiler, "on_do_publish.buildLocalMap"); + + // Get the latest robot pose in the reference frame (typ: /odom -> + // /base_link) + // so we can build the local map RELATIVE to it: + rclcpp::Duration timeout(1.0); + + try { - mp2p_icp::metric_map_t mm; - mm.layers[mp2p_icp::metric_map_t::PT_LAYER_RAW] = m_localmap_pts; - mp2p_icp_filters::apply_filter_pipeline(m_filter_pipeline, mm); - - filteredPts = mm.point_layer(m_filter_output_layer_name); + geometry_msgs::msg::TransformStamped tx; + tx = m_tf_buffer->lookupTransform( + m_frameid_reference, m_frameid_robot, tf2::TimePointZero, + tf2::durationFromSec(timeout.seconds())); + + tf2::Transform tfx; + tf2::fromMsg(tx.transform, tfx); + curRobotPose = mrpt::ros2bridge::fromROS(tfx); } - else + catch (const tf2::ExtrapolationException& ex) { - filteredPts = m_localmap_pts; + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; } - // Publish them: - if (m_pub_local_map_pointcloud.getNumSubscribers() > 0) - { - sensor_msgs::PointCloud2 msg_pts; - msg_pts.header.frame_id = m_frameid_robot; - msg_pts.header.stamp = ros::Time(obs.rbegin()->first); + RCLCPP_DEBUG(this->get_logger(), + "[onDoPublish] Building local map relative to latest robot " + "pose: %s", + curRobotPose.asString().c_str()); - auto simplPts = - std::dynamic_pointer_cast( - filteredPts); - ASSERT_(simplPts); + // For each observation: compute relative robot pose & insert obs + // into map: + for (TListObservations::const_iterator it = obs.begin(); + it != obs.end(); ++it) + { + const TInfoPerTimeStep& ipt = it->second; + + // Relative pose in the past: + mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); + relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); + + // Insert obs: + m_localmap_pts->insertObservationPtr(ipt.observation, relPose); + + } // end for + } + + // Filtering: + mrpt::maps::CPointsMap::Ptr filteredPts; + + if (!m_filter_pipeline.empty()) + { + mp2p_icp::metric_map_t mm; + mm.layers[mp2p_icp::metric_map_t::PT_LAYER_RAW] = m_localmap_pts; + mp2p_icp_filters::apply_filter_pipeline(m_filter_pipeline, mm); + + filteredPts = mm.point_layer(m_filter_output_layer_name); + } + else + { + filteredPts = m_localmap_pts; + } + + // Publish them: + if (m_pub_local_map_pointcloud->get_subscription_count() > 0) + { + sensor_msgs::msg::PointCloud2 msg_pts; + msg_pts.header.frame_id = m_frameid_robot; + msg_pts.header.stamp = rclcpp::Time(obs.rbegin()->first); + + auto simplPts = + std::dynamic_pointer_cast( + filteredPts); + ASSERT_(simplPts); + + mrpt::ros2bridge::toROS(*simplPts, msg_pts.header, msg_pts); + m_pub_local_map_pointcloud->publish(msg_pts); + } + + // Show gui: + if (m_show_gui) + { + if (!m_gui_win) + { + m_gui_win = mrpt::gui::CDisplayWindow3D::Create( + "LocalObstaclesNode", 800, 600); + mrpt::opengl::COpenGLScene::Ptr& scene = + m_gui_win->get3DSceneAndLock(); + scene->insert(mrpt::opengl::CGridPlaneXY::Create()); + scene->insert( + mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 4.0)); + + auto gl_obs = mrpt::opengl::CSetOfObjects::Create(); + gl_obs->setName("obstacles"); + scene->insert(gl_obs); + + auto gl_rawpts = mrpt::opengl::CPointCloud::Create(); + gl_rawpts->setName("raw_points"); + gl_rawpts->setPointSize(1.0); + gl_rawpts->setColor_u8(TColor(0x00ff00)); + scene->insert(gl_rawpts); + + auto gl_pts = mrpt::opengl::CPointCloud::Create(); + gl_pts->setName("final_points"); + gl_pts->setPointSize(3.0); + gl_pts->setColor_u8(TColor(0x0000ff)); + scene->insert(gl_pts); - mrpt::ros1bridge::toROS(*simplPts, msg_pts.header, msg_pts); - m_pub_local_map_pointcloud.publish(msg_pts); + m_gui_win->unlockAccess3DScene(); } - // Show gui: - if (m_show_gui) + auto& scene = m_gui_win->get3DSceneAndLock(); + auto gl_obs = mrpt::ptr_cast::from( + scene->getByName("obstacles")); + //ROS_ASSERT(!!gl_obs); + gl_obs->clear(); + + auto glRawPts = mrpt::ptr_cast::from( + scene->getByName("raw_points")); + + auto glFinalPts = mrpt::ptr_cast::from( + scene->getByName("final_points")); + + for (const auto& o : obs) { - if (!m_gui_win) - { - m_gui_win = mrpt::gui::CDisplayWindow3D::Create( - "LocalObstaclesNode", 800, 600); - mrpt::opengl::COpenGLScene::Ptr& scene = - m_gui_win->get3DSceneAndLock(); - scene->insert(mrpt::opengl::CGridPlaneXY::Create()); - scene->insert( - mrpt::opengl::stock_objects::CornerXYZSimple(1.0, 4.0)); - - auto gl_obs = mrpt::opengl::CSetOfObjects::Create(); - gl_obs->setName("obstacles"); - scene->insert(gl_obs); - - auto gl_rawpts = mrpt::opengl::CPointCloud::Create(); - gl_rawpts->setName("raw_points"); - gl_rawpts->setPointSize(1.0); - gl_rawpts->setColor_u8(TColor(0x00ff00)); - scene->insert(gl_rawpts); - - auto gl_pts = mrpt::opengl::CPointCloud::Create(); - gl_pts->setName("final_points"); - gl_pts->setPointSize(3.0); - gl_pts->setColor_u8(TColor(0x0000ff)); - scene->insert(gl_pts); - - m_gui_win->unlockAccess3DScene(); - } - - auto& scene = m_gui_win->get3DSceneAndLock(); - auto gl_obs = mrpt::ptr_cast::from( - scene->getByName("obstacles")); - ROS_ASSERT(!!gl_obs); - gl_obs->clear(); - - auto glRawPts = mrpt::ptr_cast::from( - scene->getByName("raw_points")); - - auto glFinalPts = mrpt::ptr_cast::from( - scene->getByName("final_points")); - - for (const auto& o : obs) - { - const TInfoPerTimeStep& ipt = o.second; - // Relative pose in the past: - mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); - relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); - - mrpt::opengl::CSetOfObjects::Ptr gl_axis = - mrpt::opengl::stock_objects::CornerXYZSimple(0.9, 2.0); - gl_axis->setPose(relPose); - gl_obs->insert(gl_axis); - } // end for - - glRawPts->loadFromPointsMap(m_localmap_pts.get()); - glFinalPts->loadFromPointsMap(filteredPts.get()); + const TInfoPerTimeStep& ipt = o.second; + // Relative pose in the past: + mrpt::poses::CPose3D relPose(mrpt::poses::UNINITIALIZED_POSE); + relPose.inverseComposeFrom(ipt.robot_pose, curRobotPose); - m_gui_win->unlockAccess3DScene(); - m_gui_win->repaint(); - } + mrpt::opengl::CSetOfObjects::Ptr gl_axis = + mrpt::opengl::stock_objects::CornerXYZSimple(0.9, 2.0); + gl_axis->setPose(relPose); + gl_obs->insert(gl_axis); + } // end for - } // onDoPublish + glRawPts->loadFromPointsMap(m_localmap_pts.get()); + glFinalPts->loadFromPointsMap(filteredPts.get()); -} + m_gui_win->unlockAccess3DScene(); + m_gui_win->repaint(); + } + +} // onDoPublish void LocalObstaclesNode::on_new_sensor_laser_2d(const sensor_msgs::msg::LaserScan::SharedPtr& scan) { + CTimeLoggerEntry tle(m_profiler, "on_new_sensor_laser_2d"); + rclcpp::Duration timeout(1.0); + + geometry_msgs::msg::TransformStamped sensorOnRobot; + try + { + CTimeLoggerEntry tle2( + m_profiler, "onNewSensor_Laser2D.lookupTransform_sensor"); + sensorOnRobot = m_tf_buffer->lookupTransform(m_frameid_robot, + scan->header.frame_id, scan->header.stamp, timeout); + } + catch(const tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } + + const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() { + tf2::Transform tx; + tf2::fromMsg(sensorOnRobot.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); + + // In MRPT, CObservation2DRangeScan holds both: sensor data + + // relative pose: + auto obsScan = CObservation2DRangeScan::Create(); + mrpt::ros2bridge::fromROS(*scan, sensorOnRobot_mrpt, *obsScan); + + RCLCPP_DEBUG(this->get_logger(), + "[onNewSensor_Laser2D] %u rays, sensor pose on robot %s", + static_cast(obsScan->getScanSize()), + sensorOnRobot_mrpt.asString().c_str()); + + // Get sensor timestamp: + auto stamp = scan->header.stamp; + const double timestamp = stamp.sec + static_cast(stamp.nanosec) / 1e9; + + // Get robot pose at that time in the reference frame, typ: /odom -> + // /base_link + mrpt::poses::CPose3D robotPose; + try + { + CTimeLoggerEntry tle3( + m_profiler, "onNewSensor_Laser2D.lookupTransform_robot"); + + geometry_msgs::msg::TransformStamped robotTfStamp; + try + { + robotTfStamp = m_tf_buffer->lookupTransform( + m_frameid_reference, m_frameid_robot, scan->header.stamp, + timeout); + } + catch (const tf2::ExtrapolationException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } -} + robotPose = [&]() { + tf2::Transform tx; + tf2::fromMsg(robotTfStamp.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); + + RCLCPP_DEBUG(this->get_logger(), + "[onNewSensor_Laser2D] robot pose %s", + robotPose.asString().c_str()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } + + // Insert into the observation history: + TInfoPerTimeStep ipt; + ipt.observation = obsScan; + ipt.robot_pose = robotPose; + + m_hist_obs_mtx.lock(); + m_hist_obs.insert( + m_hist_obs.end(), TListObservations::value_type(timestamp, ipt)); + m_hist_obs_mtx.unlock(); + +} // end on_new_sensor_laser_2d void LocalObstaclesNode::on_new_sensor_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr& pts) { + CTimeLoggerEntry tle(m_profiler, "on_new_sensor_pointcloud"); + + rclcpp::Duration timeout(1.0); + // Get the relative position of the sensor wrt the robot: + geometry_msgs::msg::TransformStamped sensorOnRobot; + try + { + CTimeLoggerEntry tle2( + m_profiler, "on_new_sensor_pointcloud.lookupTransform_sensor"); + + sensorOnRobot = m_tf_buffer->lookupTransform( + m_frameid_robot, pts->header.frame_id, pts->header.stamp, + timeout); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(),"%s", ex.what()); + return; + } + + // Convert data to MRPT format: + const mrpt::poses::CPose3D sensorOnRobot_mrpt = [&]() { + tf2::Transform tx; + tf2::fromMsg(sensorOnRobot.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); + + // In MRPT, CObservationPointCloud holds both: sensor data + + // relative pose: + auto obsPts = CObservationPointCloud::Create(); + const auto ptsMap = mrpt::maps::CSimplePointsMap::Create(); + obsPts->pointcloud = ptsMap; + obsPts->sensorPose = sensorOnRobot_mrpt; + mrpt::ros2bridge::fromROS(*pts, *ptsMap); + + RCLCPP_DEBUG(this->get_logger(), + "[on_new_sensor_pointcloud] %u points, sensor pose on robot %s", + static_cast(ptsMap->size()), + sensorOnRobot_mrpt.asString().c_str()); + + // Get sensor timestamp: + auto stamp = pts->header.stamp; + const double timestamp = stamp.sec + static_cast(stamp.nanosec) / 1e9; + + // Get robot pose at that time in the reference frame, typ: /odom -> + // /base_link + mrpt::poses::CPose3D robotPose; + try + { + CTimeLoggerEntry tle3( + m_profiler, "onNewSensor_pointcloud.lookupTransform_robot"); + + geometry_msgs::msg::TransformStamped robotTfStamp; + try + { + robotTfStamp = m_tf_buffer->lookupTransform( + m_frameid_reference, m_frameid_robot, pts->header.stamp, + timeout); + } + catch (const tf2::ExtrapolationException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } -} - -template -size_t LocalObstaclesNode::subscribe_to_multiple_topics(const std::string& lstTopics, - std::vector::SharedPtr>& subscriptions, - CallbackMethodType callback) -{ - -} - + robotPose = [&]() { + tf2::Transform tx; + tf2::fromMsg(robotTfStamp.transform, tx); + return mrpt::ros2bridge::fromROS(tx); + }(); + + RCLCPP_DEBUG(this->get_logger(), + "[onNewSensor_pointcloud] robot pose %s", + robotPose.asString().c_str()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "%s", ex.what()); + return; + } + + // Insert into the observation history: + TInfoPerTimeStep ipt; + ipt.observation = obsPts; + ipt.robot_pose = robotPose; + + m_hist_obs_mtx.lock(); + m_hist_obs.insert( + m_hist_obs.end(), TListObservations::value_type(timestamp, ipt)); + m_hist_obs_mtx.unlock(); +} //end on_new_sensor_pointcloud + +// read params from parameter server void LocalObstaclesNode::read_parameters() { + this->declare_parameter("show_gui", false); + this->get_parameter("show_gui", m_show_gui); + RCLCPP_INFO(this->get_logger(), "show_gui: %b", m_show_gui); + + this->declare_parameter("frameid_reference", "odom"); + this->get_parameter("frameid_reference", m_frameid_reference); + RCLCPP_INFO(this->get_logger(), "frameid_reference: %s", m_frameid_reference.c_str()); + this->declare_parameter("frameid_robot", "base_link"); + this->get_parameter("frameid_robot", m_frameid_robot); + RCLCPP_INFO(this->get_logger(), "frameid_robot: %s", m_frameid_robot.c_str()); + + this->declare_parameter("time_window", 0.20); + this->get_parameter("time_window", m_time_window); + RCLCPP_INFO(this->get_logger(), "time_window: %f", m_time_window); + + this->declare_parameter("publish_period", 0.05); + this->get_parameter("publish_period", m_publish_period); + RCLCPP_INFO(this->get_logger(), "publish_period: %f", m_publish_period); + + this->declare_parameter("topic_local_map_pointcloud", "local_map_pointcloud"); + this->get_parameter("topic_local_map_pointcloud", m_topic_local_map_pointcloud); + RCLCPP_INFO(this->get_logger(), "topic_local_map_pointcloud: %s",m_topic_local_map_pointcloud.c_str()); + + this->declare_parameter("topics_source_2dscan", "scan, laser1"); + this->get_parameter("topics_source_2dscan", m_topics_source_2dscan); + RCLCPP_INFO(this->get_logger(), "topics_source_2dscan: %s", m_topics_source_2dscan.c_str()); + + this->declare_parameter("topics_source_pointclouds", ""); + this->get_parameter("topics_source_pointclouds", m_topics_source_pointclouds); + RCLCPP_INFO(this->get_logger(), "topics_source_pointclouds: %s", m_topics_source_pointclouds.c_str()); + + this->declare_parameter("filter_yaml_file", ""); + this->get_parameter("filter_yaml_file", m_filter_yaml_file); + RCLCPP_INFO(this->get_logger(), "filter_yaml_file: %s", m_filter_yaml_file.c_str()); + if(!m_filter_yaml_file.empty()) + { + //create CConfigFile and convert into yaml and pass to filter pipeline + //m_filter_pipeline = mp2p_icp_filters::filter_pipeline_from_yaml(fil); + } + + this->declare_parameter("filter_output_layer_name", ""); + this->get_parameter("filter_output_layer_name", m_filter_output_layer_name); + RCLCPP_INFO(this->get_logger(), "filter_output_layer_name: %s", m_filter_output_layer_name.c_str()); + } int main(int argc, char ** argv) From bf7eff7e2a0a4019db3da9acd0be0c96bf199997 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Tue, 11 Jul 2023 22:41:39 -0700 Subject: [PATCH 05/12] progress: mrpt_local_obstacles --- .../mrpt_local_obstacles_node.hpp | 3 + .../launch/demo_with_mvsim.launch.py | 68 +++++++++++++++++++ .../src/mrpt_local_obstacles_node.cpp | 18 ++--- 3 files changed, 81 insertions(+), 8 deletions(-) create mode 100644 mrpt_local_obstacles/launch/demo_with_mvsim.launch.py diff --git a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp index d5fa031..3760a45 100644 --- a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp +++ b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp @@ -32,6 +32,8 @@ /* mrpt deps*/ #include +#include +#include #include #include #include @@ -47,6 +49,7 @@ #include #include #include +#include #include /* ros2 deps */ diff --git a/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py b/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py new file mode 100644 index 0000000..93ef561 --- /dev/null +++ b/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py @@ -0,0 +1,68 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + + lidar_topic_name_arg = DeclareLaunchArgument( + 'lidar_topic_name', + default_value='laser1,laser2' + ) + points_topic_name_arg = DeclareLaunchArgument( + 'points_topic_name', + default_value='/fake_obstacles' + ) + show_gui_arg = DeclareLaunchArgument( + 'show_gui', + default_value='True' + ) + time_window_arg = DeclareLaunchArgument( + 'time_window', + default_value='0.2' + ) + filter_yaml_file_arg = DeclareLaunchArgument( + 'filter_yaml_file', + default_value='' + ) + filter_output_layer_name_arg = DeclareLaunchArgument( + 'filter_output_layer_name', + default_value='' + ) + + # Node: Local obstacles builder + mrpt_local_obstacles_node = Node( + package='mrpt_local_obstacles', + executable='mrpt_local_obstacles_node', + name='mrpt_local_obstacles_node', + output='screen', + parameters=[ + {'source_topics_2dscan': LaunchConfiguration('lidar_topic_name')}, + {'source_topics_pointclouds': LaunchConfiguration('points_topic_name')}, + {'show_gui': LaunchConfiguration('show_gui')}, + {'filter_yaml_file': LaunchConfiguration('filter_yaml_file')}, + {'filter_output_layer_name': LaunchConfiguration('filter_output_layer_name')}, + {'time_window': LaunchConfiguration('time_window')}, + {'topic_local_map_pointcloud': '/local_map_pointcloud'}, + ], + ) + + mvsim_node = Node( + package='mvsim', + executable ='mvsim', + name='mvsim_node', + output='screen', + parameters=[], + ) + + return LaunchDescription([ + lidar_topic_name_arg, + points_topic_name_arg, + show_gui_arg, + time_window_arg, + filter_yaml_file_arg, + filter_output_layer_name_arg, + mrpt_local_obstacles_node + #mvsim_node + ]) diff --git a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp index b25c558..b86207d 100644 --- a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp +++ b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp @@ -449,21 +449,23 @@ void LocalObstaclesNode::read_parameters() this->get_parameter("topic_local_map_pointcloud", m_topic_local_map_pointcloud); RCLCPP_INFO(this->get_logger(), "topic_local_map_pointcloud: %s",m_topic_local_map_pointcloud.c_str()); - this->declare_parameter("topics_source_2dscan", "scan, laser1"); - this->get_parameter("topics_source_2dscan", m_topics_source_2dscan); - RCLCPP_INFO(this->get_logger(), "topics_source_2dscan: %s", m_topics_source_2dscan.c_str()); + this->declare_parameter("source_topics_2dscan", "scan, laser1"); + this->get_parameter("source_topics_2dscan", m_topics_source_2dscan); + RCLCPP_INFO(this->get_logger(), "source_topics_2dscan: %s", m_topics_source_2dscan.c_str()); - this->declare_parameter("topics_source_pointclouds", ""); - this->get_parameter("topics_source_pointclouds", m_topics_source_pointclouds); - RCLCPP_INFO(this->get_logger(), "topics_source_pointclouds: %s", m_topics_source_pointclouds.c_str()); + this->declare_parameter("source_topics_pointclouds", ""); + this->get_parameter("source_topics_pointclouds", m_topics_source_pointclouds); + RCLCPP_INFO(this->get_logger(), "source_topics_pointclouds: %s", m_topics_source_pointclouds.c_str()); this->declare_parameter("filter_yaml_file", ""); this->get_parameter("filter_yaml_file", m_filter_yaml_file); RCLCPP_INFO(this->get_logger(), "filter_yaml_file: %s", m_filter_yaml_file.c_str()); if(!m_filter_yaml_file.empty()) { - //create CConfigFile and convert into yaml and pass to filter pipeline - //m_filter_pipeline = mp2p_icp_filters::filter_pipeline_from_yaml(fil); + ASSERT_FILE_EXISTS_(m_filter_yaml_file); + mrpt::containers::yaml cfg; + cfg.loadFromFile(m_filter_yaml_file); + m_filter_pipeline = mp2p_icp_filters::filter_pipeline_from_yaml(cfg); } this->declare_parameter("filter_output_layer_name", ""); From bdcd63975760823954ce069d64be7b2bcc4d66ca Mon Sep 17 00:00:00 2001 From: SRai22 Date: Tue, 11 Jul 2023 23:36:12 -0700 Subject: [PATCH 06/12] WIP: reactivenav2d --- mrpt_local_obstacles/CMakeLists.txt | 10 +- mrpt_reactivenav2d/CATKIN_IGNORE | 0 mrpt_reactivenav2d/CMakeLists.txt | 131 ++-- mrpt_reactivenav2d/COLCON_IGNORE | 0 .../mrpt_reactivenav2d_node.hpp | 256 +++++++ mrpt_reactivenav2d/package.xml | 18 +- .../src/mrpt_reactivenav2d_node.cpp | 632 ++++++------------ 7 files changed, 532 insertions(+), 515 deletions(-) delete mode 100644 mrpt_reactivenav2d/CATKIN_IGNORE delete mode 100644 mrpt_reactivenav2d/COLCON_IGNORE create mode 100644 mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp diff --git a/mrpt_local_obstacles/CMakeLists.txt b/mrpt_local_obstacles/CMakeLists.txt index 71a5e42..eefbe2b 100644 --- a/mrpt_local_obstacles/CMakeLists.txt +++ b/mrpt_local_obstacles/CMakeLists.txt @@ -45,18 +45,18 @@ set(mola-common_DIR ${CMAKE_BINARY_DIR} CACHE PATH "path to mola-common-config.c add_subdirectory(submodules/mp2p_icp) -add_executable(${PROJECT_NAME} +add_executable(${PROJECT_NAME}_node src/mrpt_local_obstacles_node.cpp include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp) -target_include_directories(${PROJECT_NAME} +target_include_directories(${PROJECT_NAME}_node PUBLIC $ $ ) target_link_libraries( - ${PROJECT_NAME} + ${PROJECT_NAME}_node mrpt::maps mrpt::obs mrpt::gui @@ -65,7 +65,7 @@ target_link_libraries( ) ament_target_dependencies( - ${PROJECT_NAME} + ${PROJECT_NAME}_node "rclcpp" "rclcpp_components" "nav_msgs" @@ -76,7 +76,7 @@ ament_target_dependencies( ) -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME}_node ARCHIVE DESTINATION lib/${PROJECT_NAME} LIBRARY DESTINATION lib/${PROJECT_NAME} RUNTIME DESTINATION bin/${PROJECT_NAME} diff --git a/mrpt_reactivenav2d/CATKIN_IGNORE b/mrpt_reactivenav2d/CATKIN_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mrpt_reactivenav2d/CMakeLists.txt b/mrpt_reactivenav2d/CMakeLists.txt index 0f18089..811ef12 100644 --- a/mrpt_reactivenav2d/CMakeLists.txt +++ b/mrpt_reactivenav2d/CMakeLists.txt @@ -1,27 +1,34 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.5) project(mrpt_reactivenav2d) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - actionlib - actionlib_msgs - dynamic_reconfigure - geometry_msgs - roscpp - tf2 - tf2_ros - tf2_geometry_msgs - visualization_msgs -) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros) +find_package(tf2_geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(actionlib) +find_package(actionlib_msgs) + ## System dependencies are found with CMake's conventions -find_package(mrpt-ros1bridge REQUIRED) +find_package(mrpt-ros2bridge REQUIRED) find_package(mrpt-nav REQUIRED) find_package(mrpt-kinematics REQUIRED) message(STATUS "MRPT_VERSION: ${MRPT_VERSION}") +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() if (CMAKE_COMPILER_IS_GNUCXX) # High level of warnings. @@ -37,60 +44,42 @@ IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") add_compile_options(-O3) ENDIF() -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES mrpt_reactivenav2d - CATKIN_DEPENDS - actionlib - actionlib_msgs - dynamic_reconfigure - geometry_msgs - roscpp - tf2 - tf2_ros - tf2_geometry_msgs - visualization_msgs -# DEPENDS mrpt -) - ########### ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(mrpt_reactivenav2d -# src/${PROJECT_NAME}/mrpt_reactivenav2d.cpp -# ) - ## Declare a cpp executable -add_executable(mrpt_reactivenav2d_node src/mrpt_reactivenav2d_node.cpp) +add_executable(${PROJECT_NAME}_node + src/mrpt_reactivenav2d_node.cpp + include/${PROJECT_NAME}/mrpt_reactivenav2d_node.hpp +) -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(mrpt_reactivenav2d_node mrpt_reactivenav2d_generate_messages_cpp) +target_include_directories(${PROJECT_NAME}_node + PUBLIC + $ + $ +) ## Specify libraries to link a library or executable target against -target_link_libraries(mrpt_reactivenav2d_node - ${catkin_LIBRARIES} +target_link_libraries(${PROJECT_NAME}_node mrpt::nav mrpt::kinematics - mrpt::ros1bridge + mrpt::ros2bridge +) + +ament_target_dependencies( + ${PROJECT_NAME}_node + "rclcpp" + "rclcpp_components" + "nav_msgs" + "sensor_msgs" + "geometry_msgs" + "actionlib" + "actionlib_msgs" + "tf2" + "tf2_ros" + "tf2_geometry_msgs" + "visualization_msgs" ) ############# @@ -102,19 +91,31 @@ target_link_libraries(mrpt_reactivenav2d_node ## Mark executables and/or libraries for installation -install(TARGETS mrpt_reactivenav2d_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME}_node + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin/${PROJECT_NAME} ) ## Mark other files for installation (e.g. launch and bag files, etc.) install(DIRECTORY launch tutorial - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) ############# ## Testing ## ############# +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/mrpt_reactivenav2d/COLCON_IGNORE b/mrpt_reactivenav2d/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp new file mode 100644 index 0000000..870e19e --- /dev/null +++ b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp @@ -0,0 +1,256 @@ +/*********************************************************************************** + * Revised BSD License * + * Copyright (c) 2014-2023, Jose-Luis Blanco * + * All rights reserved. * + * * + * 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 Vienna University of Technology 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 Markus Bader 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace mrpt::nav; +using mrpt::maps::CSimplePointsMap; +using namespace mrpt::system; +using namespace mrpt::config; + +// The ROS node +class ReactiveNav2DNode: public rclcpp::Node +{ + public: + /* Ctor*/ + explicit ReactiveNav2DNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + /* Dtor*/ + ~ReactiveNav2DNode(){} + + private: + // methods + void read_parameters(); + void navigate_to(const mrpt::math::TPose2D& target); + void on_do_navigation(); + void on_goal_received(const geometry_msgs::msg::PoseStamped::SharedPtr& trg_ptr); + void on_local_obstacles(const sensor_msgs::msg::PointCloud::SharedPtr& obs); + void on_set_robot_shape(const geometry_msgs::msg::Polygon::SharedPtr& newShape); + + CTimeLogger m_profiler; + TAuxInitializer m_auxinit; //!< Just to make sure ROS is init first + ros::NodeHandle m_nh; //!< The node handle + ros::NodeHandle m_localn; //!< "~" + + /** @name ROS pubs/subs + * @{ */ + ros::Subscriber m_sub_nav_goal; + ros::Subscriber m_sub_local_obs; + ros::Subscriber m_sub_robot_shape; + ros::Publisher m_pub_cmd_vel; + + tf2_ros::Buffer m_tf_buffer; + tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; + /** @} */ + + bool m_1st_time_init; //!< Reactive initialization done? + double m_target_allowed_distance; + double m_nav_period; + + std::string m_pub_topic_reactive_nav_goal; + std::string m_sub_topic_local_obstacles; + std::string m_sub_topic_robot_shape; + + std::string m_frameid_reference; + std::string m_frameid_robot; + + bool m_save_nav_log; + + ros::Timer m_timer_run_nav; + + CSimplePointsMap m_last_obstacles; + std::mutex m_last_obstacles_cs; + + struct MyReactiveInterface : public mrpt::nav::CRobot2NavInterface + { + ReactiveNav2DNode& m_parent; + + MyReactiveInterface(ReactiveNav2DNode& parent) : m_parent(parent) {} + + /** Get the current pose and speeds of the robot. + * \param curPose Current robot pose. + * \param curV Current linear speed, in meters per second. + * \param curW Current angular speed, in radians per second. + * \return false on any error. + */ + bool getCurrentPoseAndSpeeds( + mrpt::math::TPose2D& curPose, mrpt::math::TTwist2D& curVel, + mrpt::system::TTimeStamp& timestamp, + mrpt::math::TPose2D& curOdometry, std::string& frame_id) override + { + double curV, curW; + + CTimeLoggerEntry tle( + m_parent.m_profiler, "getCurrentPoseAndSpeeds"); + + ros::Duration timeout(0.1); + + geometry_msgs::TransformStamped tfGeom; + try + { + CTimeLoggerEntry tle2( + m_parent.m_profiler, + "getCurrentPoseAndSpeeds.lookupTransform_sensor"); + + tfGeom = m_parent.m_tf_buffer.lookupTransform( + m_parent.m_frameid_reference, m_parent.m_frameid_robot, + ros::Time(0), timeout); + } + catch (const tf2::TransformException& ex) + { + ROS_ERROR("%s", ex.what()); + return false; + } + + tf2::Transform txRobotPose; + tf2::fromMsg(tfGeom.transform, txRobotPose); + + const mrpt::poses::CPose3D curRobotPose = + mrpt::ros1bridge::fromROS(txRobotPose); + + timestamp = mrpt::ros1bridge::fromROS(tfGeom.header.stamp); + + // Explicit 3d->2d to confirm we know we're losing information + curPose = mrpt::poses::CPose2D(curRobotPose).asTPose(); + curOdometry = curPose; + + curV = curW = 0; + MRPT_TODO("Retrieve current speeds from /odom topic?"); + ROS_DEBUG( + "[getCurrentPoseAndSpeeds] Latest pose: %s", + curPose.asString().c_str()); + + // From local to global: + curVel = mrpt::math::TTwist2D(curV, .0, curW).rotated(curPose.phi); + + return true; + } + + /** Change the instantaneous speeds of robot. + * \param v Linear speed, in meters per second. + * \param w Angular speed, in radians per second. + * \return false on any error. + */ + bool changeSpeeds( + const mrpt::kinematics::CVehicleVelCmd& vel_cmd) override + { + using namespace mrpt::kinematics; + const CVehicleVelCmd_DiffDriven* vel_cmd_diff_driven = + dynamic_cast(&vel_cmd); + ASSERT_(vel_cmd_diff_driven); + + const double v = vel_cmd_diff_driven->lin_vel; + const double w = vel_cmd_diff_driven->ang_vel; + ROS_DEBUG( + "changeSpeeds: v=%7.4f m/s w=%8.3f deg/s", v, + w * 180.0f / M_PI); + geometry_msgs::Twist cmd; + cmd.linear.x = v; + cmd.angular.z = w; + m_parent.m_pub_cmd_vel.publish(cmd); + return true; + } + + bool stop(bool isEmergency) override + { + mrpt::kinematics::CVehicleVelCmd_DiffDriven vel_cmd; + vel_cmd.lin_vel = 0; + vel_cmd.ang_vel = 0; + return changeSpeeds(vel_cmd); + } + + /** Start the watchdog timer of the robot platform, if any. + * \param T_ms Period, in ms. + * \return false on any error. */ + virtual bool startWatchdog(float T_ms) override { return true; } + /** Stop the watchdog timer. + * \return false on any error. */ + virtual bool stopWatchdog() override { return true; } + /** Return the current set of obstacle points. + * \return false on any error. */ + bool senseObstacles( + CSimplePointsMap& obstacles, + mrpt::system::TTimeStamp& timestamp) override + { + timestamp = mrpt::system::now(); + std::lock_guard csl(m_parent.m_last_obstacles_cs); + obstacles = m_parent.m_last_obstacles; + + MRPT_TODO("TODO: Check age of obstacles!"); + return true; + } + + mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() override + { + return getStopCmd(); + } + mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() override + { + mrpt::kinematics::CVehicleVelCmd::Ptr ret = + mrpt::kinematics::CVehicleVelCmd::Ptr( + new mrpt::kinematics::CVehicleVelCmd_DiffDriven); + ret->setToStop(); + return ret; + } + + void sendNavigationStartEvent() override {} + void sendNavigationEndEvent() override {} + void sendNavigationEndDueToErrorEvent() override {} + void sendWaySeemsBlockedEvent() override {} + }; + + MyReactiveInterface m_reactive_if; + + CReactiveNavigationSystem m_reactive_nav_engine; + std::mutex m_reactive_nav_engine_cs; +}; \ No newline at end of file diff --git a/mrpt_reactivenav2d/package.xml b/mrpt_reactivenav2d/package.xml index 4731ad1..dd7fe51 100644 --- a/mrpt_reactivenav2d/package.xml +++ b/mrpt_reactivenav2d/package.xml @@ -1,33 +1,35 @@ - + mrpt_reactivenav2d 1.0.3 Reactive navigation for 2D robots using MRPT navigation algorithms (TP-Space) Jose-Luis Blanco-Claraco + Shravan S Rai + + Jose-Luis Blanco-Claraco BSD https://wiki.ros.org/mrpt_reactivenav2d - Jose-Luis Blanco-Claraco - - - catkin + ament_cmake mrpt2 actionlib actionlib_msgs - dynamic_reconfigure + rclcpp + rclcpp_components + nav_msgs + sensor_msgs geometry_msgs - roscpp tf2 tf2_ros tf2_geometry_msgs visualization_msgs - + ament_cmake diff --git a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp index 424cc10..49a0873 100644 --- a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp +++ b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp @@ -1,479 +1,237 @@ -/*********************************************************************************** - * Revised BSD License * - * Copyright (c) 2014-2015, Jose-Luis Blanco * - * All rights reserved. * - * * - * 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 Vienna University of Technology 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 Markus Bader 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" - -using namespace mrpt::nav; -using mrpt::maps::CSimplePointsMap; -using namespace mrpt::system; -using namespace mrpt::config; - -// The ROS node -class ReactiveNav2DNode +#include "mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp" + + +/** Constructor: Inits ROS system */ +ReactiveNav2DNode::ReactiveNav2DNode(int argc, char** argv) + : m_auxinit(argc, argv), + m_nh(), + m_localn("~"), + m_1st_time_init(false), + m_target_allowed_distance(0.40f), + m_nav_period(0.100), + m_pub_topic_reactive_nav_goal("reactive_nav_goal"), + m_sub_topic_local_obstacles("local_map_pointcloud"), + m_sub_topic_robot_shape(""), + m_frameid_reference("map"), + m_frameid_robot("base_link"), + m_save_nav_log(false), + m_reactive_if(*this), + m_reactive_nav_engine(m_reactive_if) { - private: - struct TAuxInitializer + // Load params: + std::string cfg_file_reactive; + m_localn.param( + "cfg_file_reactive", cfg_file_reactive, cfg_file_reactive); + m_localn.param( + "target_allowed_distance", m_target_allowed_distance, + m_target_allowed_distance); + m_localn.param("nav_period", m_nav_period, m_nav_period); + m_localn.param( + "frameid_reference", m_frameid_reference, m_frameid_reference); + m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot); + m_localn.param( + "topic_robot_shape", m_sub_topic_robot_shape, + m_sub_topic_robot_shape); + m_localn.param("save_nav_log", m_save_nav_log, m_save_nav_log); + + ROS_ASSERT(m_nav_period > 0); + ROS_ASSERT_MSG( + !cfg_file_reactive.empty(), + "Mandatory param 'cfg_file_reactive' is missing!"); + ROS_ASSERT_MSG( + mrpt::system::fileExists(cfg_file_reactive), + "Config file not found: '%s'", cfg_file_reactive.c_str()); + + m_reactive_nav_engine.enableLogFile(m_save_nav_log); + + // Load reactive config: + // ---------------------------------------------------- + try { - TAuxInitializer(int argc, char** argv) - { - ros::init(argc, argv, "mrpt_reactivenav2d"); - } - }; - - CTimeLogger m_profiler; - TAuxInitializer m_auxinit; //!< Just to make sure ROS is init first - ros::NodeHandle m_nh; //!< The node handle - ros::NodeHandle m_localn; //!< "~" - - /** @name ROS pubs/subs - * @{ */ - ros::Subscriber m_sub_nav_goal; - ros::Subscriber m_sub_local_obs; - ros::Subscriber m_sub_robot_shape; - ros::Publisher m_pub_cmd_vel; - - tf2_ros::Buffer m_tf_buffer; - tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; - /** @} */ - - bool m_1st_time_init; //!< Reactive initialization done? - double m_target_allowed_distance; - double m_nav_period; - - std::string m_pub_topic_reactive_nav_goal; - std::string m_sub_topic_local_obstacles; - std::string m_sub_topic_robot_shape; - - std::string m_frameid_reference; - std::string m_frameid_robot; - - bool m_save_nav_log; - - ros::Timer m_timer_run_nav; - - CSimplePointsMap m_last_obstacles; - std::mutex m_last_obstacles_cs; - - struct MyReactiveInterface : public mrpt::nav::CRobot2NavInterface + CConfigFile cfgFil(cfg_file_reactive); + m_reactive_nav_engine.loadConfigFile(cfgFil); + } + catch (std::exception& e) { - ReactiveNav2DNode& m_parent; - - MyReactiveInterface(ReactiveNav2DNode& parent) : m_parent(parent) {} - - /** Get the current pose and speeds of the robot. - * \param curPose Current robot pose. - * \param curV Current linear speed, in meters per second. - * \param curW Current angular speed, in radians per second. - * \return false on any error. - */ - bool getCurrentPoseAndSpeeds( - mrpt::math::TPose2D& curPose, mrpt::math::TTwist2D& curVel, - mrpt::system::TTimeStamp& timestamp, - mrpt::math::TPose2D& curOdometry, std::string& frame_id) override - { - double curV, curW; - - CTimeLoggerEntry tle( - m_parent.m_profiler, "getCurrentPoseAndSpeeds"); - - ros::Duration timeout(0.1); + ROS_ERROR( + "Exception initializing reactive navigation engine:\n%s", + e.what()); + throw; + } - geometry_msgs::TransformStamped tfGeom; - try - { - CTimeLoggerEntry tle2( - m_parent.m_profiler, - "getCurrentPoseAndSpeeds.lookupTransform_sensor"); + // load robot shape: (1) default, (2) via params, (3) via topic + // ---------------------------------------------------------------- + // m_reactive_nav_engine.changeRobotShape(); - tfGeom = m_parent.m_tf_buffer.lookupTransform( - m_parent.m_frameid_reference, m_parent.m_frameid_robot, - ros::Time(0), timeout); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR("%s", ex.what()); - return false; - } + // Init this subscriber first so we know asap the desired robot shape, + // if provided via a topic: + if (!m_sub_topic_robot_shape.empty()) + { + m_sub_robot_shape = m_nh.subscribe( + m_sub_topic_robot_shape, 1, + &ReactiveNav2DNode::onRosSetRobotShape, this); + ROS_INFO( + "Params say robot shape will arrive via topic '%s'... waiting " + "3 seconds for it.", + m_sub_topic_robot_shape.c_str()); + ros::Duration(3.0).sleep(); + for (size_t i = 0; i < 100; i++) ros::spinOnce(); + ROS_INFO("Wait done."); + } - tf2::Transform txRobotPose; - tf2::fromMsg(tfGeom.transform, txRobotPose); + // Init ROS publishers: + // ----------------------- + m_pub_cmd_vel = m_nh.advertise("cmd_vel", 1); + + // Init ROS subs: + // ----------------------- + // "/reactive_nav_goal", "/move_base_simple/goal" ( + // geometry_msgs/PoseStamped ) + m_sub_nav_goal = m_nh.subscribe( + m_pub_topic_reactive_nav_goal, 1, + &ReactiveNav2DNode::onRosGoalReceived, this); + m_sub_local_obs = m_nh.subscribe( + m_sub_topic_local_obstacles, 1, + &ReactiveNav2DNode::onRosLocalObstacles, this); + + // Init timers: + // ---------------------------------------------------- + m_timer_run_nav = m_nh.createTimer( + ros::Duration(m_nav_period), &ReactiveNav2DNode::onDoNavigation, + this); + +} // end ctor + +/** + * @brief Issue a navigation command + * @param target The target location + */ +void ReactiveNav2DNode::navigate_to(const mrpt::math::TPose2D& target) +{ + ROS_INFO( + "[navigateTo] Starting navigation to %s", + target.asString().c_str()); - const mrpt::poses::CPose3D curRobotPose = - mrpt::ros1bridge::fromROS(txRobotPose); + CAbstractPTGBasedReactive::TNavigationParamsPTG navParams; - timestamp = mrpt::ros1bridge::fromROS(tfGeom.header.stamp); + CAbstractNavigator::TargetInfo target_info; + target_info.target_coords.x = target.x; + target_info.target_coords.y = target.y; + target_info.targetAllowedDistance = m_target_allowed_distance; + target_info.targetIsRelative = false; - // Explicit 3d->2d to confirm we know we're losing information - curPose = mrpt::poses::CPose2D(curRobotPose).asTPose(); - curOdometry = curPose; + // API for multiple waypoints: + //... + // navParams.multiple_targets.push_back(target_info); - curV = curW = 0; - MRPT_TODO("Retrieve current speeds from /odom topic?"); - ROS_DEBUG( - "[getCurrentPoseAndSpeeds] Latest pose: %s", - curPose.asString().c_str()); + // API for single targets: + navParams.target = target_info; - // From local to global: - curVel = mrpt::math::TTwist2D(curV, .0, curW).rotated(curPose.phi); + // Optional: restrict the PTGs to use + // navParams.restrict_PTG_indices.push_back(1); - return true; - } + { + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.navigate(&navParams); + } +} - /** Change the instantaneous speeds of robot. - * \param v Linear speed, in meters per second. - * \param w Angular speed, in radians per second. - * \return false on any error. - */ - bool changeSpeeds( - const mrpt::kinematics::CVehicleVelCmd& vel_cmd) override +/** Callback: On run navigation */ +void ReactiveNav2DNode::on_do_navigation() +{ + // 1st time init: + // ---------------------------------------------------- + if (!m_1st_time_init) + { + m_1st_time_init = true; + ROS_INFO( + "[ReactiveNav2DNode] Initializing reactive navigation " + "engine..."); { - using namespace mrpt::kinematics; - const CVehicleVelCmd_DiffDriven* vel_cmd_diff_driven = - dynamic_cast(&vel_cmd); - ASSERT_(vel_cmd_diff_driven); - - const double v = vel_cmd_diff_driven->lin_vel; - const double w = vel_cmd_diff_driven->ang_vel; - ROS_DEBUG( - "changeSpeeds: v=%7.4f m/s w=%8.3f deg/s", v, - w * 180.0f / M_PI); - geometry_msgs::Twist cmd; - cmd.linear.x = v; - cmd.angular.z = w; - m_parent.m_pub_cmd_vel.publish(cmd); - return true; + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.initialize(); } + ROS_INFO( + "[ReactiveNav2DNode] Reactive navigation engine init done!"); + } - bool stop(bool isEmergency) override - { - mrpt::kinematics::CVehicleVelCmd_DiffDriven vel_cmd; - vel_cmd.lin_vel = 0; - vel_cmd.ang_vel = 0; - return changeSpeeds(vel_cmd); - } + CTimeLoggerEntry tle(m_profiler, "onDoNavigation"); - /** Start the watchdog timer of the robot platform, if any. - * \param T_ms Period, in ms. - * \return false on any error. */ - virtual bool startWatchdog(float T_ms) override { return true; } - /** Stop the watchdog timer. - * \return false on any error. */ - virtual bool stopWatchdog() override { return true; } - /** Return the current set of obstacle points. - * \return false on any error. */ - bool senseObstacles( - CSimplePointsMap& obstacles, - mrpt::system::TTimeStamp& timestamp) override - { - timestamp = mrpt::system::now(); - std::lock_guard csl(m_parent.m_last_obstacles_cs); - obstacles = m_parent.m_last_obstacles; + m_reactive_nav_engine.navigationStep(); +} - MRPT_TODO("TODO: Check age of obstacles!"); - return true; - } +void ReactiveNav2DNode::on_goal_received(const geometry_msgs::msg::PoseStamped::SharedPtr& trg_ptr) +{ + geometry_msgs::PoseStamped trg = *trg_ptr; - mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() override - { - return getStopCmd(); - } - mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() override - { - mrpt::kinematics::CVehicleVelCmd::Ptr ret = - mrpt::kinematics::CVehicleVelCmd::Ptr( - new mrpt::kinematics::CVehicleVelCmd_DiffDriven); - ret->setToStop(); - return ret; - } + ROS_INFO( + "Nav target received via topic sub: (%.03f,%.03f, %.03fdeg) " + "[frame_id=%s]", + trg.pose.position.x, trg.pose.position.y, + trg.pose.orientation.z * 180.0 / M_PI, trg.header.frame_id.c_str()); - void sendNavigationStartEvent() override {} - void sendNavigationEndEvent() override {} - void sendNavigationEndDueToErrorEvent() override {} - void sendWaySeemsBlockedEvent() override {} - }; - - MyReactiveInterface m_reactive_if; - - CReactiveNavigationSystem m_reactive_nav_engine; - std::mutex m_reactive_nav_engine_cs; - - public: - /** Constructor: Inits ROS system */ - ReactiveNav2DNode(int argc, char** argv) - : m_auxinit(argc, argv), - m_nh(), - m_localn("~"), - m_1st_time_init(false), - m_target_allowed_distance(0.40f), - m_nav_period(0.100), - m_pub_topic_reactive_nav_goal("reactive_nav_goal"), - m_sub_topic_local_obstacles("local_map_pointcloud"), - m_sub_topic_robot_shape(""), - m_frameid_reference("map"), - m_frameid_robot("base_link"), - m_save_nav_log(false), - m_reactive_if(*this), - m_reactive_nav_engine(m_reactive_if) + // Convert to the "m_frameid_reference" frame of coordinates: + if (trg.header.frame_id != m_frameid_reference) { - // Load params: - std::string cfg_file_reactive; - m_localn.param( - "cfg_file_reactive", cfg_file_reactive, cfg_file_reactive); - m_localn.param( - "target_allowed_distance", m_target_allowed_distance, - m_target_allowed_distance); - m_localn.param("nav_period", m_nav_period, m_nav_period); - m_localn.param( - "frameid_reference", m_frameid_reference, m_frameid_reference); - m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot); - m_localn.param( - "topic_robot_shape", m_sub_topic_robot_shape, - m_sub_topic_robot_shape); - m_localn.param("save_nav_log", m_save_nav_log, m_save_nav_log); - - ROS_ASSERT(m_nav_period > 0); - ROS_ASSERT_MSG( - !cfg_file_reactive.empty(), - "Mandatory param 'cfg_file_reactive' is missing!"); - ROS_ASSERT_MSG( - mrpt::system::fileExists(cfg_file_reactive), - "Config file not found: '%s'", cfg_file_reactive.c_str()); - - m_reactive_nav_engine.enableLogFile(m_save_nav_log); - - // Load reactive config: - // ---------------------------------------------------- + ros::Duration timeout(0.2); try { - CConfigFile cfgFil(cfg_file_reactive); - m_reactive_nav_engine.loadConfigFile(cfgFil); - } - catch (std::exception& e) - { - ROS_ERROR( - "Exception initializing reactive navigation engine:\n%s", - e.what()); - throw; - } + geometry_msgs::TransformStamped ref_to_trgFrame = + m_tf_buffer.lookupTransform( + trg.header.frame_id, m_frameid_reference, ros::Time(0), + timeout); - // load robot shape: (1) default, (2) via params, (3) via topic - // ---------------------------------------------------------------- - // m_reactive_nav_engine.changeRobotShape(); - - // Init this subscriber first so we know asap the desired robot shape, - // if provided via a topic: - if (!m_sub_topic_robot_shape.empty()) - { - m_sub_robot_shape = m_nh.subscribe( - m_sub_topic_robot_shape, 1, - &ReactiveNav2DNode::onRosSetRobotShape, this); - ROS_INFO( - "Params say robot shape will arrive via topic '%s'... waiting " - "3 seconds for it.", - m_sub_topic_robot_shape.c_str()); - ros::Duration(3.0).sleep(); - for (size_t i = 0; i < 100; i++) ros::spinOnce(); - ROS_INFO("Wait done."); + tf2::doTransform(trg, trg, ref_to_trgFrame); } - - // Init ROS publishers: - // ----------------------- - m_pub_cmd_vel = m_nh.advertise("cmd_vel", 1); - - // Init ROS subs: - // ----------------------- - // "/reactive_nav_goal", "/move_base_simple/goal" ( - // geometry_msgs/PoseStamped ) - m_sub_nav_goal = m_nh.subscribe( - m_pub_topic_reactive_nav_goal, 1, - &ReactiveNav2DNode::onRosGoalReceived, this); - m_sub_local_obs = m_nh.subscribe( - m_sub_topic_local_obstacles, 1, - &ReactiveNav2DNode::onRosLocalObstacles, this); - - // Init timers: - // ---------------------------------------------------- - m_timer_run_nav = m_nh.createTimer( - ros::Duration(m_nav_period), &ReactiveNav2DNode::onDoNavigation, - this); - - } // end ctor - - /** - * @brief Issue a navigation command - * @param target The target location - */ - void navigateTo(const mrpt::math::TPose2D& target) - { - ROS_INFO( - "[navigateTo] Starting navigation to %s", - target.asString().c_str()); - - CAbstractPTGBasedReactive::TNavigationParamsPTG navParams; - - CAbstractNavigator::TargetInfo target_info; - target_info.target_coords.x = target.x; - target_info.target_coords.y = target.y; - target_info.targetAllowedDistance = m_target_allowed_distance; - target_info.targetIsRelative = false; - - // API for multiple waypoints: - //... - // navParams.multiple_targets.push_back(target_info); - - // API for single targets: - navParams.target = target_info; - - // Optional: restrict the PTGs to use - // navParams.restrict_PTG_indices.push_back(1); - + catch (const tf2::TransformException& ex) { - std::lock_guard csl(m_reactive_nav_engine_cs); - m_reactive_nav_engine.navigate(&navParams); + ROS_ERROR("%s", ex.what()); + return; } } - /** Callback: On run navigation */ - void onDoNavigation(const ros::TimerEvent&) - { - // 1st time init: - // ---------------------------------------------------- - if (!m_1st_time_init) - { - m_1st_time_init = true; - ROS_INFO( - "[ReactiveNav2DNode] Initializing reactive navigation " - "engine..."); - { - std::lock_guard csl(m_reactive_nav_engine_cs); - m_reactive_nav_engine.initialize(); - } - ROS_INFO( - "[ReactiveNav2DNode] Reactive navigation engine init done!"); - } + this->navigateTo(mrpt::math::TPose2D( + trg.pose.position.x, trg.pose.position.y, trg.pose.orientation.z)); +} - CTimeLoggerEntry tle(m_profiler, "onDoNavigation"); +void ReactiveNav2DNode::on_local_obstacles(const sensor_msgs::msg::PointCloud::SharedPtr& obs) +{ + std::lock_guard csl(m_last_obstacles_cs); + mrpt::ros2bridge::fromROS(*obs, m_last_obstacles); + // ROS_DEBUG("Local obstacles received: %u points", static_cast(m_last_obstacles.size()) ); +} - m_reactive_nav_engine.navigationStep(); - } +void ReactiveNav2DNode::on_set_robot_shape(const geometry_msgs::msg::Polygon::SharedPtr& newShape) +{ + ROS_INFO_STREAM( + "[onRosSetRobotShape] Robot shape received via topic: " + << *newShape); - void onRosGoalReceived(const geometry_msgs::PoseStampedConstPtr& trg_ptr) + mrpt::math::CPolygon poly; + poly.resize(newShape->points.size()); + for (size_t i = 0; i < newShape->points.size(); i++) { - geometry_msgs::PoseStamped trg = *trg_ptr; - - ROS_INFO( - "Nav target received via topic sub: (%.03f,%.03f, %.03fdeg) " - "[frame_id=%s]", - trg.pose.position.x, trg.pose.position.y, - trg.pose.orientation.z * 180.0 / M_PI, trg.header.frame_id.c_str()); - - // Convert to the "m_frameid_reference" frame of coordinates: - if (trg.header.frame_id != m_frameid_reference) - { - ros::Duration timeout(0.2); - try - { - geometry_msgs::TransformStamped ref_to_trgFrame = - m_tf_buffer.lookupTransform( - trg.header.frame_id, m_frameid_reference, ros::Time(0), - timeout); - - tf2::doTransform(trg, trg, ref_to_trgFrame); - } - catch (const tf2::TransformException& ex) - { - ROS_ERROR("%s", ex.what()); - return; - } - } - - this->navigateTo(mrpt::math::TPose2D( - trg.pose.position.x, trg.pose.position.y, trg.pose.orientation.z)); + poly[i].x = newShape->points[i].x; + poly[i].y = newShape->points[i].y; } - void onRosLocalObstacles(const sensor_msgs::PointCloudConstPtr& obs) { - std::lock_guard csl(m_last_obstacles_cs); - mrpt::ros1bridge::fromROS(*obs, m_last_obstacles); - // ROS_DEBUG("Local obstacles received: %u points", static_cast(m_last_obstacles.size()) ); + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.changeRobotShape(poly); } +} - void onRosSetRobotShape(const geometry_msgs::PolygonConstPtr& newShape) - { - ROS_INFO_STREAM( - "[onRosSetRobotShape] Robot shape received via topic: " - << *newShape); - mrpt::math::CPolygon poly; - poly.resize(newShape->points.size()); - for (size_t i = 0; i < newShape->points.size(); i++) - { - poly[i].x = newShape->points[i].x; - poly[i].y = newShape->points[i].y; - } +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); - { - std::lock_guard csl(m_reactive_nav_engine_cs); - m_reactive_nav_engine.changeRobotShape(poly); - } - } + auto node = std::make_shared(); -}; // end class + rclcpp::spin(node); -int main(int argc, char** argv) -{ - ReactiveNav2DNode the_node(argc, argv); - ros::spin(); - return 0; -} + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file From 2e7e6d79d7d94616044cb8c14157c846992d96af Mon Sep 17 00:00:00 2001 From: SRai22 Date: Fri, 14 Jul 2023 03:34:47 -0700 Subject: [PATCH 07/12] reactiveNav2d code port complete --- mrpt_reactivenav2d/CMakeLists.txt | 6 +- .../mrpt_reactivenav2d_node.hpp | 83 ++-- mrpt_reactivenav2d/package.xml | 3 +- .../src/mrpt_reactivenav2d_node.cpp | 364 +++++++++++++----- 4 files changed, 323 insertions(+), 133 deletions(-) diff --git a/mrpt_reactivenav2d/CMakeLists.txt b/mrpt_reactivenav2d/CMakeLists.txt index 811ef12..e3af50f 100644 --- a/mrpt_reactivenav2d/CMakeLists.txt +++ b/mrpt_reactivenav2d/CMakeLists.txt @@ -8,12 +8,11 @@ find_package(rclcpp_components REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs) find_package(sensor_msgs REQUIRED) +find_package(mrpt_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros) find_package(tf2_geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) -find_package(actionlib) -find_package(actionlib_msgs) ## System dependencies are found with CMake's conventions @@ -74,8 +73,7 @@ ament_target_dependencies( "nav_msgs" "sensor_msgs" "geometry_msgs" - "actionlib" - "actionlib_msgs" + "mrpt_msgs" "tf2" "tf2_ros" "tf2_geometry_msgs" diff --git a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp index 870e19e..fb76d83 100644 --- a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp +++ b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp @@ -29,28 +29,35 @@ ** * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ***********************************************************************************/ - +#pragma once #include #include #include #include #include -#include +#include +#include +#include #include #include #include #include +#include +#include +#include #include #include -#include +#include #include #include #include #include #include #include +#include +#include #include @@ -74,42 +81,54 @@ class ReactiveNav2DNode: public rclcpp::Node void navigate_to(const mrpt::math::TPose2D& target); void on_do_navigation(); void on_goal_received(const geometry_msgs::msg::PoseStamped::SharedPtr& trg_ptr); - void on_local_obstacles(const sensor_msgs::msg::PointCloud::SharedPtr& obs); + void on_local_obstacles(const sensor_msgs::msg::PointCloud2::SharedPtr& obs); void on_set_robot_shape(const geometry_msgs::msg::Polygon::SharedPtr& newShape); + void on_odometry_received(const nav_msgs::msg::Odometry::SharedPtr& odom); + void update_waypoint_sequence(const mrpt_msgs::msg::WaypointSequence::SharedPtr& wp); + void on_waypoint_seq_received(const mrpt_msgs::msg::WaypointSequence::SharedPtr& wps); - CTimeLogger m_profiler; - TAuxInitializer m_auxinit; //!< Just to make sure ROS is init first - ros::NodeHandle m_nh; //!< The node handle - ros::NodeHandle m_localn; //!< "~" + private: /** @name ROS pubs/subs * @{ */ - ros::Subscriber m_sub_nav_goal; - ros::Subscriber m_sub_local_obs; - ros::Subscriber m_sub_robot_shape; - ros::Publisher m_pub_cmd_vel; - - tf2_ros::Buffer m_tf_buffer; - tf2_ros::TransformListener m_tf_listener{m_tf_buffer}; + rclcpp::Subscription::SharedPtr m_sub_odometry; + rclcpp::Subscription::SharedPtr m_sub_wp_seq; + rclcpp::Subscription::SharedPtr m_sub_nav_goal; + rclcpp::Subscription::SharedPtr m_sub_local_obs; + rclcpp::Subscription::SharedPtr m_sub_robot_shape; + rclcpp::Publisher::SharedPtr m_pub_cmd_vel; + + std::shared_ptr m_tf_buffer; + std::shared_ptr m_tf_listener; /** @} */ + CTimeLogger m_profiler; bool m_1st_time_init; //!< Reactive initialization done? double m_target_allowed_distance; double m_nav_period; - std::string m_pub_topic_reactive_nav_goal; - std::string m_sub_topic_local_obstacles; - std::string m_sub_topic_robot_shape; + std::string m_sub_topic_reactive_nav_goal = "reactive_nav_goal"; + std::string m_sub_topic_local_obstacles = "local_map_pointcloud"; + std::string m_sub_topic_robot_shape{}; + std::string m_sub_topic_wp_seq = "reactive_nav_waypoints"; + std::string m_sub_topic_odometry = "odom"; + + std::string m_pub_topic_cmd_vel = "cmd_vel"; + + std::string m_frameid_reference = "map"; + std::string m_frameid_robot = "base_link"; - std::string m_frameid_reference; - std::string m_frameid_robot; + std::string m_plugin_file; + std::string m_cfg_file_reactive; bool m_save_nav_log; - ros::Timer m_timer_run_nav; + rclcpp::TimerBase::SharedPtr m_timer_run_nav; + mrpt::obs::CObservationOdometry m_odometry; CSimplePointsMap m_last_obstacles; std::mutex m_last_obstacles_cs; + std::mutex m_odometry_cs; struct MyReactiveInterface : public mrpt::nav::CRobot2NavInterface { @@ -133,22 +152,22 @@ class ReactiveNav2DNode: public rclcpp::Node CTimeLoggerEntry tle( m_parent.m_profiler, "getCurrentPoseAndSpeeds"); - ros::Duration timeout(0.1); + rclcpp::Duration timeout(0.1); - geometry_msgs::TransformStamped tfGeom; + geometry_msgs::msg::TransformStamped tfGeom; try { CTimeLoggerEntry tle2( m_parent.m_profiler, "getCurrentPoseAndSpeeds.lookupTransform_sensor"); - tfGeom = m_parent.m_tf_buffer.lookupTransform( + tfGeom = m_parent.m_tf_buffer->lookupTransform( m_parent.m_frameid_reference, m_parent.m_frameid_robot, - ros::Time(0), timeout); + tf2::TimePointZero, tf2::durationFromSec(timeout.seconds())); } catch (const tf2::TransformException& ex) { - ROS_ERROR("%s", ex.what()); + RCLCPP_ERROR(m_parent.get_logger(), "%s", ex.what()); return false; } @@ -156,9 +175,9 @@ class ReactiveNav2DNode: public rclcpp::Node tf2::fromMsg(tfGeom.transform, txRobotPose); const mrpt::poses::CPose3D curRobotPose = - mrpt::ros1bridge::fromROS(txRobotPose); + mrpt::ros2bridge::fromROS(txRobotPose); - timestamp = mrpt::ros1bridge::fromROS(tfGeom.header.stamp); + timestamp = mrpt::ros2bridge::fromROS(tfGeom.header.stamp); // Explicit 3d->2d to confirm we know we're losing information curPose = mrpt::poses::CPose2D(curRobotPose).asTPose(); @@ -166,7 +185,7 @@ class ReactiveNav2DNode: public rclcpp::Node curV = curW = 0; MRPT_TODO("Retrieve current speeds from /odom topic?"); - ROS_DEBUG( + RCLCPP_DEBUG(m_parent.get_logger(), "[getCurrentPoseAndSpeeds] Latest pose: %s", curPose.asString().c_str()); @@ -191,13 +210,13 @@ class ReactiveNav2DNode: public rclcpp::Node const double v = vel_cmd_diff_driven->lin_vel; const double w = vel_cmd_diff_driven->ang_vel; - ROS_DEBUG( + RCLCPP_DEBUG(m_parent.get_logger(), "changeSpeeds: v=%7.4f m/s w=%8.3f deg/s", v, w * 180.0f / M_PI); - geometry_msgs::Twist cmd; + geometry_msgs::msg::Twist cmd; cmd.linear.x = v; cmd.angular.z = w; - m_parent.m_pub_cmd_vel.publish(cmd); + m_parent.m_pub_cmd_vel->publish(cmd); return true; } diff --git a/mrpt_reactivenav2d/package.xml b/mrpt_reactivenav2d/package.xml index dd7fe51..2c01eab 100644 --- a/mrpt_reactivenav2d/package.xml +++ b/mrpt_reactivenav2d/package.xml @@ -17,13 +17,12 @@ mrpt2 - actionlib - actionlib_msgs rclcpp rclcpp_components nav_msgs sensor_msgs geometry_msgs + mrpt_msgs tf2 tf2_ros tf2_geometry_msgs diff --git a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp index 49a0873..fe872e2 100644 --- a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp +++ b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp @@ -1,114 +1,235 @@ +#include +#include #include "mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp" +using namespace mrpt::nav; +using mrpt::maps::CSimplePointsMap; +using namespace mrpt::system; +using namespace mrpt::config; /** Constructor: Inits ROS system */ -ReactiveNav2DNode::ReactiveNav2DNode(int argc, char** argv) - : m_auxinit(argc, argv), - m_nh(), - m_localn("~"), - m_1st_time_init(false), - m_target_allowed_distance(0.40f), - m_nav_period(0.100), - m_pub_topic_reactive_nav_goal("reactive_nav_goal"), - m_sub_topic_local_obstacles("local_map_pointcloud"), - m_sub_topic_robot_shape(""), - m_frameid_reference("map"), - m_frameid_robot("base_link"), - m_save_nav_log(false), - m_reactive_if(*this), - m_reactive_nav_engine(m_reactive_if) +ReactiveNav2DNode::ReactiveNav2DNode(const rclcpp::NodeOptions& options) +: Node("mrpt_reactivenav2d", options) +, m_1st_time_init(false) +, m_target_allowed_distance(0.40f) +, m_nav_period(0.100) +, m_save_nav_log(false) +, m_reactive_if(*this) +, m_reactive_nav_engine(m_reactive_if) { - // Load params: - std::string cfg_file_reactive; - m_localn.param( - "cfg_file_reactive", cfg_file_reactive, cfg_file_reactive); - m_localn.param( - "target_allowed_distance", m_target_allowed_distance, - m_target_allowed_distance); - m_localn.param("nav_period", m_nav_period, m_nav_period); - m_localn.param( - "frameid_reference", m_frameid_reference, m_frameid_reference); - m_localn.param("frameid_robot", m_frameid_robot, m_frameid_robot); - m_localn.param( - "topic_robot_shape", m_sub_topic_robot_shape, - m_sub_topic_robot_shape); - m_localn.param("save_nav_log", m_save_nav_log, m_save_nav_log); - - ROS_ASSERT(m_nav_period > 0); - ROS_ASSERT_MSG( - !cfg_file_reactive.empty(), - "Mandatory param 'cfg_file_reactive' is missing!"); - ROS_ASSERT_MSG( - mrpt::system::fileExists(cfg_file_reactive), - "Config file not found: '%s'", cfg_file_reactive.c_str()); + // Load params + read_parameters(); - m_reactive_nav_engine.enableLogFile(m_save_nav_log); + assert(m_nav_period > 0); - // Load reactive config: - // ---------------------------------------------------- - try + if (m_cfg_file_reactive.empty()) { - CConfigFile cfgFil(cfg_file_reactive); - m_reactive_nav_engine.loadConfigFile(cfgFil); + RCLCPP_ERROR(this->get_logger(), + "Mandatory param 'cfg_file_reactive' is missing!"); + throw; } - catch (std::exception& e) + + if (!mrpt::system::fileExists(m_cfg_file_reactive)) { - ROS_ERROR( - "Exception initializing reactive navigation engine:\n%s", - e.what()); + RCLCPP_ERROR(this->get_logger(), + "Config file not found: %s", m_cfg_file_reactive.c_str()); throw; } + m_reactive_nav_engine.enableLogFile(m_save_nav_log); + + // Load reactive config: + // ---------------------------------------------------- + if(!m_cfg_file_reactive.empty()) + { + try + { + CConfigFile cfgFil(m_cfg_file_reactive); + m_reactive_nav_engine.loadConfigFile(cfgFil); + } + catch (std::exception& e) + { + RCLCPP_ERROR(this->get_logger(), + "Exception initializing reactive navigation engine:\n%s", + e.what()); + throw; + } + } // load robot shape: (1) default, (2) via params, (3) via topic // ---------------------------------------------------------------- // m_reactive_nav_engine.changeRobotShape(); // Init this subscriber first so we know asap the desired robot shape, - // if provided via a topic: + // if provided via a topic: if (!m_sub_topic_robot_shape.empty()) { - m_sub_robot_shape = m_nh.subscribe( + m_sub_robot_shape = this->create_subscription( m_sub_topic_robot_shape, 1, - &ReactiveNav2DNode::onRosSetRobotShape, this); - ROS_INFO( - "Params say robot shape will arrive via topic '%s'... waiting " - "3 seconds for it.", + [this](const geometry_msgs::msg::Polygon::SharedPtr poly) + {this->on_set_robot_shape(poly);}); + + RCLCPP_INFO(this->get_logger(), + "Params say robot shape will arrive via topic '%s'... waiting 3 seconds for it.", m_sub_topic_robot_shape.c_str()); - ros::Duration(3.0).sleep(); - for (size_t i = 0; i < 100; i++) ros::spinOnce(); - ROS_INFO("Wait done."); + + // Use rate object to implement sleep + rclcpp::Rate rate(1); // 1 Hz + for (int i = 0; i < 3; i++) + { + rclcpp::spin_some(this->get_node_base_interface()); + rate.sleep(); + } + RCLCPP_INFO(this->get_logger(), "Wait done."); + } + else + { + // Load robot shape: 1/2 polygon + // --------------------------------------------- + CConfigFile c(m_cfg_file_reactive); + std::string s = "CReactiveNavigationSystem"; + + std::vector xs, ys; + c.read_vector( + s, "RobotModel_shape2D_xs", std::vector(), xs, false); + c.read_vector( + s, "RobotModel_shape2D_ys", std::vector(), ys, false); + ASSERTMSG_( + xs.size() == ys.size(), + "Config parameters `RobotModel_shape2D_xs` and " + "`RobotModel_shape2D_ys` " + "must have the same length!"); + if (!xs.empty()) + { + mrpt::math::CPolygon poly; + poly.resize(xs.size()); + for (size_t i = 0; i < xs.size(); i++) + { + poly[i].x = xs[i]; + poly[i].y = ys[i]; + } + + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.changeRobotShape(poly); + } + + // Load robot shape: 2/2 circle + // --------------------------------------------- + if (const double robot_radius = c.read_double( + s, "RobotModel_circular_shape_radius", -1.0, false); + robot_radius > 0) + { + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.changeRobotCircularShapeRadius( + robot_radius); + } } // Init ROS publishers: // ----------------------- - m_pub_cmd_vel = m_nh.advertise("cmd_vel", 1); + m_pub_cmd_vel = this->create_publisher(m_pub_topic_cmd_vel, 1); // Init ROS subs: // ----------------------- - // "/reactive_nav_goal", "/move_base_simple/goal" ( - // geometry_msgs/PoseStamped ) - m_sub_nav_goal = m_nh.subscribe( - m_pub_topic_reactive_nav_goal, 1, - &ReactiveNav2DNode::onRosGoalReceived, this); - m_sub_local_obs = m_nh.subscribe( - m_sub_topic_local_obstacles, 1, - &ReactiveNav2DNode::onRosLocalObstacles, this); - - // Init timers: + m_sub_odometry = this->create_subscription( + m_sub_topic_odometry, 1, + [this](const nav_msgs::msg::Odometry::SharedPtr odom) + {this->on_odometry_received(odom);}); + + m_sub_wp_seq = this->create_subscription( + m_sub_topic_wp_seq, 1, + [this](const mrpt_msgs::msg::WaypointSequence::SharedPtr msg) + {this->on_waypoint_seq_received(msg);}); + + m_sub_nav_goal = this->create_subscription( + m_sub_topic_reactive_nav_goal, 1, + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { this->on_goal_received(msg); }); + + m_sub_local_obs = this->create_subscription( + m_sub_topic_local_obstacles, 1, + [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { this->on_local_obstacles(msg); }); + + // Init tf buffers + // ---------------------------------------------------- + m_tf_buffer = std::make_shared(this->get_clock()); + m_tf_listener = std::make_shared(*m_tf_buffer); + + // Init timer: // ---------------------------------------------------- - m_timer_run_nav = m_nh.createTimer( - ros::Duration(m_nav_period), &ReactiveNav2DNode::onDoNavigation, - this); + m_timer_run_nav = this->create_wall_timer( + std::chrono::duration(m_nav_period), + [this]() { this->on_do_navigation(); }); + } // end ctor +void ReactiveNav2DNode::read_parameters() +{ + this->declare_parameter("cfg_file_reactive", "reactive2d_config.ini"); + this->get_parameter("cfg_file_reactive", m_cfg_file_reactive); + RCLCPP_INFO(this->get_logger(), "cfg_file_reactive %s", m_cfg_file_reactive.c_str()); + + this->declare_parameter("target_allowed_distance", 0.40); + this->get_parameter("target_allowed_distance", m_target_allowed_distance); + RCLCPP_INFO(this->get_logger(), "target_allowed_distance: %f", m_target_allowed_distance); + + this->declare_parameter("nav_period", 0.10); + this->get_parameter("nav_period", m_nav_period); + RCLCPP_INFO(this->get_logger(), "nav_period: %f", m_nav_period); + + this->declare_parameter("frameid_reference", "odom"); + this->get_parameter("frameid_reference", m_frameid_reference); + RCLCPP_INFO(this->get_logger(), "frameid_reference: %s", m_frameid_reference.c_str()); + + this->declare_parameter("frameid_robot", "base_link"); + this->get_parameter("frameid_robot", m_frameid_robot); + RCLCPP_INFO(this->get_logger(), "frameid_robot: %s", m_frameid_robot.c_str()); + + this->declare_parameter("topic_wp_seq", "waypointsequence"); + this->get_parameter("topic_wp_seq", m_sub_topic_wp_seq); + RCLCPP_INFO(this->get_logger(), "topic_wp_seq: %s", m_sub_topic_wp_seq.c_str()); + + this->declare_parameter("topic_odometry", "/odometry"); + this->get_parameter("topic_odometry", m_sub_topic_odometry); + RCLCPP_INFO(this->get_logger(), "topic_odometry: %s", m_sub_topic_odometry.c_str()); + + this->declare_parameter("topic_cmd_vel", "/cmd_vel"); + this->get_parameter("topic_cmd_vel", m_pub_topic_cmd_vel); + RCLCPP_INFO(this->get_logger(), "topic_cmd_vel: %s", m_pub_topic_cmd_vel.c_str()); + + this->declare_parameter("topic_obstacles", "/pointcloud"); + this->get_parameter("topic_obstacles", m_sub_topic_local_obstacles); + RCLCPP_INFO(this->get_logger(), "topic_obstacles: %s", m_sub_topic_local_obstacles.c_str()); + + this->declare_parameter("save_nav_log", false); + this->get_parameter("save_nav_log", m_save_nav_log); + RCLCPP_INFO(this->get_logger(), "save_nav_log: %b", m_save_nav_log); + + this->declare_parameter("ptg_plugin_files", ""); + this->get_parameter("ptg_plugin_files", m_plugin_file); + RCLCPP_INFO(this->get_logger(), "ptg_plugin_files: %s", m_plugin_file.c_str()); + + if (!m_plugin_file.empty()) + { + RCLCPP_INFO_STREAM( this->get_logger(), + "About to load plugins: " << m_plugin_file); + std::string errorMsgs; + if (!mrpt::system::loadPluginModules(m_plugin_file, errorMsgs)) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Error loading rnav plugins: " << errorMsgs); + } + RCLCPP_INFO_STREAM(this->get_logger(), "Pluginns loaded OK."); + } +} + /** * @brief Issue a navigation command * @param target The target location */ void ReactiveNav2DNode::navigate_to(const mrpt::math::TPose2D& target) { - ROS_INFO( + RCLCPP_INFO(this->get_logger(), "[navigateTo] Starting navigation to %s", target.asString().c_str()); @@ -120,10 +241,6 @@ void ReactiveNav2DNode::navigate_to(const mrpt::math::TPose2D& target) target_info.targetAllowedDistance = m_target_allowed_distance; target_info.targetIsRelative = false; - // API for multiple waypoints: - //... - // navParams.multiple_targets.push_back(target_info); - // API for single targets: navParams.target = target_info; @@ -144,27 +261,81 @@ void ReactiveNav2DNode::on_do_navigation() if (!m_1st_time_init) { m_1st_time_init = true; - ROS_INFO( + RCLCPP_INFO( this->get_logger(), "[ReactiveNav2DNode] Initializing reactive navigation " "engine..."); { std::lock_guard csl(m_reactive_nav_engine_cs); m_reactive_nav_engine.initialize(); } - ROS_INFO( + RCLCPP_INFO(this->get_logger(), "[ReactiveNav2DNode] Reactive navigation engine init done!"); } - CTimeLoggerEntry tle(m_profiler, "onDoNavigation"); - + CTimeLoggerEntry tle(m_profiler, "on_do_navigation"); + // Main nav loop (in whatever state nav is: IDLE, NAVIGATING, etc.) m_reactive_nav_engine.navigationStep(); } +void ReactiveNav2DNode::on_odometry_received(const nav_msgs::msg::Odometry::SharedPtr& msg) +{ + std::lock_guard csl(m_odometry_cs); + tf2::Quaternion quat( + msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + m_odometry.odometry = mrpt::poses::CPose2D( + msg->pose.pose.position.x, msg->pose.pose.position.y, yaw); + + m_odometry.velocityLocal.vx = msg->twist.twist.linear.x; + m_odometry.velocityLocal.vy = msg->twist.twist.linear.y; + m_odometry.velocityLocal.omega = msg->twist.twist.angular.z; + m_odometry.hasVelocities = true; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Odometry updated"); +} + +void ReactiveNav2DNode::on_waypoint_seq_received(const mrpt_msgs::msg::WaypointSequence::SharedPtr& wps) +{ + update_waypoint_sequence(std::move(wps)); +} + +void ReactiveNav2DNode::update_waypoint_sequence(const mrpt_msgs::msg::WaypointSequence::SharedPtr& msg) +{ + mrpt::nav::TWaypointSequence wps; + + for (const auto& wp : msg->waypoints) + { + tf2::Quaternion quat( + wp.target.orientation.x, wp.target.orientation.y, + wp.target.orientation.z, wp.target.orientation.w); + tf2::Matrix3x3 mat(quat); + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + auto waypoint = mrpt::nav::TWaypoint( + wp.target.position.x, wp.target.position.y, wp.allowed_distance, + wp.allow_skip); + + if (yaw == yaw && !wp.ignore_heading) // regular number, not NAN + waypoint.target_heading = yaw; + + wps.waypoints.push_back(waypoint); + } + + RCLCPP_INFO_STREAM(this->get_logger(), "New navigateWaypoints() command"); + { + std::lock_guard csl(m_reactive_nav_engine_cs); + m_reactive_nav_engine.navigateWaypoints(wps); + } +} + void ReactiveNav2DNode::on_goal_received(const geometry_msgs::msg::PoseStamped::SharedPtr& trg_ptr) { - geometry_msgs::PoseStamped trg = *trg_ptr; + geometry_msgs::msg::PoseStamped trg = *trg_ptr; - ROS_INFO( + RCLCPP_INFO(this->get_logger(), "Nav target received via topic sub: (%.03f,%.03f, %.03fdeg) " "[frame_id=%s]", trg.pose.position.x, trg.pose.position.y, @@ -173,40 +344,43 @@ void ReactiveNav2DNode::on_goal_received(const geometry_msgs::msg::PoseStamped:: // Convert to the "m_frameid_reference" frame of coordinates: if (trg.header.frame_id != m_frameid_reference) { - ros::Duration timeout(0.2); + rclcpp::Duration timeout(0.2); try { - geometry_msgs::TransformStamped ref_to_trgFrame = - m_tf_buffer.lookupTransform( - trg.header.frame_id, m_frameid_reference, ros::Time(0), - timeout); + geometry_msgs::msg::TransformStamped ref_to_trgFrame = + m_tf_buffer->lookupTransform( + trg.header.frame_id, m_frameid_reference, + tf2::TimePointZero, tf2::durationFromSec(timeout.seconds())); tf2::doTransform(trg, trg, ref_to_trgFrame); } catch (const tf2::TransformException& ex) { - ROS_ERROR("%s", ex.what()); + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); return; } } - this->navigateTo(mrpt::math::TPose2D( + this->navigate_to(mrpt::math::TPose2D( trg.pose.position.x, trg.pose.position.y, trg.pose.orientation.z)); } -void ReactiveNav2DNode::on_local_obstacles(const sensor_msgs::msg::PointCloud::SharedPtr& obs) +void ReactiveNav2DNode::on_local_obstacles(const sensor_msgs::msg::PointCloud2::SharedPtr& obs) { std::lock_guard csl(m_last_obstacles_cs); mrpt::ros2bridge::fromROS(*obs, m_last_obstacles); - // ROS_DEBUG("Local obstacles received: %u points", static_cast(m_last_obstacles.size()) ); + RCLCPP_DEBUG(this->get_logger(), + "Local obstacles received: %u points", static_cast(m_last_obstacles.size())); } void ReactiveNav2DNode::on_set_robot_shape(const geometry_msgs::msg::Polygon::SharedPtr& newShape) { - ROS_INFO_STREAM( - "[onRosSetRobotShape] Robot shape received via topic: " - << *newShape); + RCLCPP_INFO_STREAM(this->get_logger(), "[onSetRobotShape] Robot shape received via topic:"); + for (const auto& point : newShape->points) + { + RCLCPP_INFO_STREAM(this->get_logger(), "Point - x: " << point.x << ", y: " << point.y << ", z: " << point.z); + } mrpt::math::CPolygon poly; poly.resize(newShape->points.size()); From f7f99aab293bfb87c82bf9b89a110c555f63143a Mon Sep 17 00:00:00 2001 From: SRai22 Date: Fri, 14 Jul 2023 03:41:58 -0700 Subject: [PATCH 08/12] minor fixes --- .../mrpt_local_obstacles_node.hpp | 34 +++++++++---------- .../src/mrpt_local_obstacles_node.cpp | 10 +++--- 2 files changed, 23 insertions(+), 21 deletions(-) diff --git a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp index 3760a45..84c8d0d 100644 --- a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp +++ b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp @@ -102,23 +102,23 @@ class LocalObstaclesNode: public rclcpp::Node CallbackMethodType callback) { size_t num_subscriptions = 0; - // std::vector lstSources; - // mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources); - - // // Error handling: check if lstSources is empty - // if (lstSources.empty()) - // { - // RCLCPP_ERROR(this->get_logger(), "List of topics is empty."); - // return 0; // Return early with 0 subscriptions - // } - // for (const auto& source : lstSources) - // { - // const auto sub = this->create_subscription(source, 1, callback) - // subscriptions.push_back(sub); // 1 is the queue size - // num_subscriptions++; - // } - - // // Return the number of subscriptions created + std::vector lstSources; + mrpt::system::tokenize(lstTopics, " ,\t\n", lstSources); + + // Error handling: check if lstSources is empty + if (lstSources.empty()) + { + RCLCPP_ERROR(this->get_logger(), "List of topics is empty."); + return 0; // Return early with 0 subscriptions + } + for (const auto& source : lstSources) + { + const auto sub = this->create_subscription(source, 1, callback); + subscriptions.push_back(sub); // 1 is the queue size + num_subscriptions++; + } + + // Return the number of subscriptions created return num_subscriptions; } diff --git a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp index b86207d..26677e2 100644 --- a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp +++ b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp @@ -23,11 +23,13 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) size_t nSubsTotal = 0; nSubsTotal += subscribe_to_multiple_topics( m_topics_source_2dscan, m_subs_2dlaser, - std::bind(&LocalObstaclesNode::on_new_sensor_laser_2d, this, std::placeholders::_1)); + [this](const sensor_msgs::msg::LaserScan::SharedPtr scan) + {this-> on_new_sensor_laser_2d(scan);}); nSubsTotal += subscribe_to_multiple_topics( - m_topics_source_pointclouds, m_subs_pointclouds, - std::bind(&LocalObstaclesNode::on_new_sensor_pointcloud, this, std::placeholders::_1)); + m_topics_source_pointclouds, m_subs_pointclouds, + [this](const sensor_msgs::msg::PointCloud2::SharedPtr pts) + {this->on_new_sensor_pointcloud(pts);}); RCLCPP_INFO(this->get_logger(), "Total number of sensor subscriptions: %u", @@ -47,7 +49,7 @@ LocalObstaclesNode::LocalObstaclesNode(const rclcpp::NodeOptions& options) m_timer_publish = create_wall_timer( std::chrono::duration(m_publish_period), - std::bind(&LocalObstaclesNode::on_do_publish, this)); + [this](){this->on_do_publish();}); } //end ctor From d8d476601b7e29e0eee4dd3138402b14b1835e19 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Tue, 1 Aug 2023 16:53:19 -0700 Subject: [PATCH 09/12] Minor fixes for humble or above --- mrpt_local_obstacles/CMakeLists.txt | 4 +--- .../mrpt_local_obstacles/mrpt_local_obstacles_node.hpp | 2 ++ mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mrpt_local_obstacles/CMakeLists.txt b/mrpt_local_obstacles/CMakeLists.txt index eefbe2b..42ffafe 100644 --- a/mrpt_local_obstacles/CMakeLists.txt +++ b/mrpt_local_obstacles/CMakeLists.txt @@ -77,9 +77,7 @@ ament_target_dependencies( install(TARGETS ${PROJECT_NAME}_node - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION bin/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) install( diff --git a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp index 84c8d0d..7f194cc 100644 --- a/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp +++ b/mrpt_local_obstacles/include/mrpt_local_obstacles/mrpt_local_obstacles_node.hpp @@ -59,7 +59,9 @@ #include #include #include +#include +#include #include #include diff --git a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp index 26677e2..cafd6e1 100644 --- a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp +++ b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp @@ -98,7 +98,7 @@ void LocalObstaclesNode::on_do_publish() // Get the latest robot pose in the reference frame (typ: /odom -> // /base_link) // so we can build the local map RELATIVE to it: - rclcpp::Duration timeout(1.0); + rclcpp::Duration timeout(std::chrono::seconds(1)); try { @@ -240,7 +240,7 @@ void LocalObstaclesNode::on_do_publish() void LocalObstaclesNode::on_new_sensor_laser_2d(const sensor_msgs::msg::LaserScan::SharedPtr& scan) { CTimeLoggerEntry tle(m_profiler, "on_new_sensor_laser_2d"); - rclcpp::Duration timeout(1.0); + rclcpp::Duration timeout(std::chrono::seconds(1)); geometry_msgs::msg::TransformStamped sensorOnRobot; try @@ -332,7 +332,7 @@ void LocalObstaclesNode::on_new_sensor_pointcloud(const sensor_msgs::msg::PointC { CTimeLoggerEntry tle(m_profiler, "on_new_sensor_pointcloud"); - rclcpp::Duration timeout(1.0); + rclcpp::Duration timeout(std::chrono::seconds(1)); // Get the relative position of the sensor wrt the robot: geometry_msgs::msg::TransformStamped sensorOnRobot; try From b43abe1eb712f96c0d755a9ea145b13c59817df2 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Tue, 1 Aug 2023 16:59:32 -0700 Subject: [PATCH 10/12] Minor fixes for humble and above --- mrpt_reactivenav2d/CMakeLists.txt | 4 +--- .../include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp | 4 +++- mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp | 3 ++- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/mrpt_reactivenav2d/CMakeLists.txt b/mrpt_reactivenav2d/CMakeLists.txt index e3af50f..c7c5399 100644 --- a/mrpt_reactivenav2d/CMakeLists.txt +++ b/mrpt_reactivenav2d/CMakeLists.txt @@ -90,9 +90,7 @@ ament_target_dependencies( ## Mark executables and/or libraries for installation install(TARGETS ${PROJECT_NAME}_node - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION bin/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) ## Mark other files for installation (e.g. launch and bag files, etc.) diff --git a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp index fb76d83..9db9fdf 100644 --- a/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp +++ b/mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp @@ -60,6 +60,7 @@ #include #include +#include using namespace mrpt::nav; using mrpt::maps::CSimplePointsMap; @@ -152,7 +153,8 @@ class ReactiveNav2DNode: public rclcpp::Node CTimeLoggerEntry tle( m_parent.m_profiler, "getCurrentPoseAndSpeeds"); - rclcpp::Duration timeout(0.1); + //rclcpp::Duration timeout(0.1); + rclcpp::Duration timeout(std::chrono::milliseconds(100)); geometry_msgs::msg::TransformStamped tfGeom; try diff --git a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp index fe872e2..2c7bc7d 100644 --- a/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp +++ b/mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp @@ -344,7 +344,8 @@ void ReactiveNav2DNode::on_goal_received(const geometry_msgs::msg::PoseStamped:: // Convert to the "m_frameid_reference" frame of coordinates: if (trg.header.frame_id != m_frameid_reference) { - rclcpp::Duration timeout(0.2); + //rclcpp::Duration timeout(0.2); + rclcpp::Duration timeout(std::chrono::milliseconds(200)); try { geometry_msgs::msg::TransformStamped ref_to_trgFrame = From 7734f9b75e5c6a220f8d327663f513a31afd8593 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Mon, 7 Aug 2023 22:07:13 -0700 Subject: [PATCH 11/12] mrpt_local_obs port complete --- .../launch/demo_with_mvsim.launch.py | 32 +++++++++++-------- .../local-obstacles-decimation-filter.yaml | 15 +++++++++ .../src/mrpt_local_obstacles_node.cpp | 6 +++- 3 files changed, 39 insertions(+), 14 deletions(-) create mode 100644 mrpt_local_obstacles/launch/local-obstacles-decimation-filter.yaml diff --git a/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py b/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py index 93ef561..0b9a9b3 100644 --- a/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py +++ b/mrpt_local_obstacles/launch/demo_with_mvsim.launch.py @@ -1,18 +1,21 @@ import os from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): + lidar_topic_name_arg = DeclareLaunchArgument( 'lidar_topic_name', - default_value='laser1,laser2' + default_value='/laser1, /laser2' ) points_topic_name_arg = DeclareLaunchArgument( 'points_topic_name', - default_value='/fake_obstacles' + default_value='/particlecloud' ) show_gui_arg = DeclareLaunchArgument( 'show_gui', @@ -24,11 +27,11 @@ def generate_launch_description(): ) filter_yaml_file_arg = DeclareLaunchArgument( 'filter_yaml_file', - default_value='' + default_value= os.path.join(os.path.dirname(__file__), 'local-obstacles-decimation-filter.yaml') ) filter_output_layer_name_arg = DeclareLaunchArgument( 'filter_output_layer_name', - default_value='' + default_value='decimated' ) # Node: Local obstacles builder @@ -48,15 +51,19 @@ def generate_launch_description(): ], ) - mvsim_node = Node( - package='mvsim', - executable ='mvsim', - name='mvsim_node', - output='screen', - parameters=[], - ) + mvsim_pkg_share_dir = get_package_share_directory('mvsim') + # Finding the launch file + launch_file_name = 'demo_jackal.launch.py' + mvsim_launch_file_path = os.path.join(mvsim_pkg_share_dir, 'mvsim_tutorial', launch_file_name) + + # Check if the launch file exists + if not os.path.isfile(mvsim_launch_file_path): + raise Exception(f"Launch file '{mvsim_launch_file_path}' does not exist!") return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(mvsim_launch_file_path) + ), lidar_topic_name_arg, points_topic_name_arg, show_gui_arg, @@ -64,5 +71,4 @@ def generate_launch_description(): filter_yaml_file_arg, filter_output_layer_name_arg, mrpt_local_obstacles_node - #mvsim_node ]) diff --git a/mrpt_local_obstacles/launch/local-obstacles-decimation-filter.yaml b/mrpt_local_obstacles/launch/local-obstacles-decimation-filter.yaml new file mode 100644 index 0000000..b382e61 --- /dev/null +++ b/mrpt_local_obstacles/launch/local-obstacles-decimation-filter.yaml @@ -0,0 +1,15 @@ +- class_name: mp2p_icp_filters::FilterDecimateVoxels + params: + output_pointcloud_layer: 'decimated_pre' + voxel_filter_resolution: 0.05 # [m] + use_voxel_average: true + bounding_box_min: [ -5, -5, 0 ] + bounding_box_max: [ 10, 5, 1 ] +- class_name: mp2p_icp_filters::FilterBoundingBox + params: + input_pointcloud_layer: 'decimated_pre' + output_pointcloud_layer: 'decimated' + keep_bbox_contents: false + bounding_box_min: [ -0.22, -0.27, -1 ] + bounding_box_max: [ 0.22, 0.27, 1 ] + diff --git a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp index cafd6e1..f722d38 100644 --- a/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp +++ b/mrpt_local_obstacles/src/mrpt_local_obstacles_node.cpp @@ -1,5 +1,6 @@ #include #include "rclcpp_components/register_node_macro.hpp" +#include using namespace mrpt::system; using namespace mrpt::config; @@ -467,10 +468,13 @@ void LocalObstaclesNode::read_parameters() ASSERT_FILE_EXISTS_(m_filter_yaml_file); mrpt::containers::yaml cfg; cfg.loadFromFile(m_filter_yaml_file); + std::stringstream ss; + cfg.printAsYAML(ss); + RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str()); m_filter_pipeline = mp2p_icp_filters::filter_pipeline_from_yaml(cfg); } - this->declare_parameter("filter_output_layer_name", ""); + this->declare_parameter("filter_output_layer_name", "decimated"); this->get_parameter("filter_output_layer_name", m_filter_output_layer_name); RCLCPP_INFO(this->get_logger(), "filter_output_layer_name: %s", m_filter_output_layer_name.c_str()); From abd07ec30269d49fd618f8e80b1ee095c033d460 Mon Sep 17 00:00:00 2001 From: SRai22 Date: Mon, 7 Aug 2023 22:54:19 -0700 Subject: [PATCH 12/12] minor fixes ros2 port msgs bridge --- mrpt_msgs_bridge/CMakeLists.txt | 6 +++--- mrpt_msgs_bridge/package.xml | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/mrpt_msgs_bridge/CMakeLists.txt b/mrpt_msgs_bridge/CMakeLists.txt index d45bc45..a885c4a 100644 --- a/mrpt_msgs_bridge/CMakeLists.txt +++ b/mrpt_msgs_bridge/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(mrpt_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(marker_msgs REQUIRED) find_package(mrpt-ros2bridge REQUIRED) find_package(mrpt-obs REQUIRED) @@ -58,9 +59,7 @@ target_link_libraries(${PROJECT_NAME} # Mark executables and/or libraries for installation install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) # Mark cpp header files for installation @@ -75,6 +74,7 @@ ament_target_dependencies(${PROJECT_NAME} visualization_msgs tf2 mrpt_msgs + marker_msgs ) #ament_copyright() diff --git a/mrpt_msgs_bridge/package.xml b/mrpt_msgs_bridge/package.xml index 77f3ce6..edf47e5 100644 --- a/mrpt_msgs_bridge/package.xml +++ b/mrpt_msgs_bridge/package.xml @@ -15,18 +15,19 @@ https://wiki.ros.org/mrpt_msgs_bridge ros_environment - + ament_cmake catkin mrpt2 geometry_msgs visualization_msgs + marker_msgs mrpt_msgs tf2 - ament + ament_cmake