diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..9a6ec50 --- /dev/null +++ b/.clang-format @@ -0,0 +1,75 @@ +--- +BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false + +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true + +AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + +AllowShortBlocksOnASingleLine: true +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortCaseLabelsOnASingleLine: false + +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true + +BinPackParameters: true +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true + +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 +PenaltyBreakString: 100 +PenaltyReturnTypeOnItsOwnLine: 50 + +SpacesBeforeTrailingComments: 2 +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: + AfterCaseLabel: true + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false +... diff --git a/laser_ortho_projector/COLCON_IGNORE b/laser_ortho_projector/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/laser_scan_matcher/CMakeLists.txt b/laser_scan_matcher/CMakeLists.txt index ce20592..91e7810 100644 --- a/laser_scan_matcher/CMakeLists.txt +++ b/laser_scan_matcher/CMakeLists.txt @@ -1,72 +1,80 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10.2) project(laser_scan_matcher) # List C++ dependencies on ros packages set( ROS_CXX_DEPENDENCIES - roscpp - nodelet + rclcpp + rclcpp_components sensor_msgs - tf - pcl_ros + tf2 + tf2_ros + # pcl_ros pcl_conversions geometry_msgs - nav_msgs) + nav_msgs + Boost) -# Find catkin and all required ROS components -find_package(catkin REQUIRED COMPONENTS ${ROS_CXX_DEPENDENCIES} rostest) +# Find all required ROS2 packages +find_package(ament_cmake REQUIRED) +foreach(DEPENDENCY ${ROS_CXX_DEPENDENCIES}) + find_package(${DEPENDENCY} REQUIRED) +endforeach() + +# Suppress warnings, see https://cmake.org/cmake/help/v3.17/module/FindPackageHandleStandardArgs.html +set(FPHSA_NAME_MISMATCHED 1) find_package(PCL REQUIRED QUIET) +unset(FPHSA_NAME_MISMATCHED) # Find csm project find_package(PkgConfig) pkg_check_modules(csm REQUIRED csm) # Set include directories -include_directories(include ${catkin_INCLUDE_DIRS} ${csm_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +include_directories(include ${csm_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) link_directories(${csm_LIBRARY_DIRS}) -# Declare info that other packages need to import library generated here -catkin_package( - INCLUDE_DIRS include - LIBRARIES laser_scan_matcher - CATKIN_DEPENDS ${ROS_CXX_DEPENDENCIES} -) - #Create library -add_library(laser_scan_matcher src/laser_scan_matcher.cpp) +add_library(laser_scan_matcher SHARED src/laser_scan_matcher.cpp) #Note we don't link against pcl as we're using header-only parts of the library -target_link_libraries( laser_scan_matcher ${catkin_LIBRARIES} ${csm_LIBRARIES}) -add_dependencies(laser_scan_matcher ${csm_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(laser_scan_matcher + tf2 + tf2_ros + Boost + pcl_conversions + sensor_msgs + geometry_msgs + nav_msgs + rclcpp) +target_link_libraries( laser_scan_matcher ${csm_LIBRARIES}) -#Create nodelet -add_library(laser_scan_matcher_nodelet src/laser_scan_matcher_nodelet.cpp) -target_link_libraries(laser_scan_matcher_nodelet laser_scan_matcher) +#Create component +add_library(laser_scan_matcher_component SHARED src/laser_scan_matcher_component.cpp) +target_link_libraries(laser_scan_matcher_component laser_scan_matcher) +rclcpp_components_register_nodes(laser_scan_matcher_component "scan_tools::LaserScanMatcherComponent") #Create node add_executable(laser_scan_matcher_node src/laser_scan_matcher_node.cpp) -target_link_libraries( laser_scan_matcher_node laser_scan_matcher ) +target_link_libraries(laser_scan_matcher_node laser_scan_matcher) #Install library -install(TARGETS laser_scan_matcher laser_scan_matcher_nodelet - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) +install(TARGETS laser_scan_matcher laser_scan_matcher_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME}) #Install library includes install(DIRECTORY include/laser_scan_matcher/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) + DESTINATION include) #Install node install(TARGETS laser_scan_matcher_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - -#Install nodelet description -install(FILES laser_scan_matcher_nodelet.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + RUNTIME DESTINATION lib/${PROJECT_NAME}) #Install demo files install(DIRECTORY demo - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + DESTINATION share/${PROJECT_NAME}) -add_rostest(test/run.test) -add_rostest(test/covariance.test) +# add_rostest(test/run.test) +# add_rostest(test/covariance.test) +ament_package() \ No newline at end of file diff --git a/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h b/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h index 994074e..a7ae11a 100644 --- a/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h +++ b/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher.h @@ -38,22 +38,22 @@ #ifndef LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H #define LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H -#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 +#include +//#include #include // csm defines min and max, but Eigen complains #undef min @@ -61,130 +61,125 @@ namespace scan_tools { - class LaserScanMatcher { - public: - - LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private); - ~LaserScanMatcher(); - - private: +public: + LaserScanMatcher(const rclcpp::Node::SharedPtr& node); + ~LaserScanMatcher(); - typedef pcl::PointXYZ PointT; - typedef pcl::PointCloud PointCloudT; +private: + typedef pcl::PointXYZ PointT; + typedef pcl::PointCloud PointCloudT; - // **** ros + // **** ros - ros::NodeHandle nh_; - ros::NodeHandle nh_private_; + rclcpp::Node::SharedPtr node_; - ros::Subscriber scan_subscriber_; - ros::Subscriber cloud_subscriber_; - ros::Subscriber odom_subscriber_; - ros::Subscriber imu_subscriber_; - ros::Subscriber vel_subscriber_; + rclcpp::SubscriptionBase::SharedPtr scan_subscriber_; + rclcpp::SubscriptionBase::SharedPtr cloud_subscriber_; + rclcpp::SubscriptionBase::SharedPtr odom_subscriber_; + rclcpp::SubscriptionBase::SharedPtr imu_subscriber_; + rclcpp::SubscriptionBase::SharedPtr vel_subscriber_; - tf::TransformListener tf_listener_; - tf::TransformBroadcaster tf_broadcaster_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + tf2_ros::TransformBroadcaster tf_broadcaster_; - tf::Transform base_to_laser_; // static, cached - tf::Transform laser_to_base_; // static, cached, calculated from base_to_laser_ + tf2::Transform base_to_laser_; // static, cached + tf2::Transform laser_to_base_; // static, cached, calculated from base_to_laser_ - ros::Publisher pose_publisher_; - ros::Publisher pose_stamped_publisher_; - ros::Publisher pose_with_covariance_publisher_; - ros::Publisher pose_with_covariance_stamped_publisher_; + // TODO(JafarAbdi): Use PublisherBase .? No publish function :/ + rclcpp::Publisher::SharedPtr pose_publisher_; + rclcpp::Publisher::SharedPtr pose_stamped_publisher_; + rclcpp::Publisher::SharedPtr pose_with_covariance_publisher_; + rclcpp::Publisher::SharedPtr pose_with_covariance_stamped_publisher_; - // **** parameters + // **** parameters - std::string base_frame_; - std::string fixed_frame_; - double cloud_range_min_; - double cloud_range_max_; - double cloud_res_; - bool publish_tf_; - bool publish_pose_; - bool publish_pose_with_covariance_; - bool publish_pose_stamped_; - bool publish_pose_with_covariance_stamped_; - std::vector position_covariance_; - std::vector orientation_covariance_; + std::string base_frame_; + std::string fixed_frame_; + double cloud_range_min_; + double cloud_range_max_; + double cloud_res_; + bool publish_tf_; + bool publish_pose_; + bool publish_pose_with_covariance_; + bool publish_pose_stamped_; + bool publish_pose_with_covariance_stamped_; + std::vector position_covariance_; + std::vector orientation_covariance_; - bool use_cloud_input_; + bool use_cloud_input_; - double kf_dist_linear_; - double kf_dist_linear_sq_; - double kf_dist_angular_; + double kf_dist_linear_; + double kf_dist_linear_sq_; + double kf_dist_angular_; - // **** What predictions are available to speed up the ICP? - // 1) imu - [theta] from imu yaw angle - /imu topic - // 2) odom - [x, y, theta] from wheel odometry - /odom topic - // 3) velocity [vx, vy, vtheta], usually from ab-filter - /vel. - // If more than one is enabled, priority is imu > odom > velocity + // **** What predictions are available to speed up the ICP? + // 1) imu - [theta] from imu yaw angle - /imu topic + // 2) odom - [x, y, theta] from wheel odometry - /odom topic + // 3) velocity [vx, vy, vtheta], usually from ab-filter - /vel. + // If more than one is enabled, priority is imu > odom > velocity - bool use_imu_; - bool use_odom_; - bool use_vel_; - bool stamped_vel_; + bool use_imu_; + bool use_odom_; + bool use_vel_; + bool stamped_vel_; - // **** state variables + // **** state variables - boost::mutex mutex_; + std::mutex mutex_; - bool initialized_; - bool received_imu_; - bool received_odom_; - bool received_vel_; + bool initialized_; + bool received_imu_; + bool received_odom_; + bool received_vel_; - tf::Transform f2b_; // fixed-to-base tf (pose of base frame in fixed frame) - tf::Transform f2b_kf_; // pose of the last keyframe scan in fixed frame + tf2::Transform f2b_; // fixed-to-base tf (pose of base frame in fixed frame) + tf2::Transform f2b_kf_; // pose of the last keyframe scan in fixed frame - ros::Time last_icp_time_; + rclcpp::Time last_icp_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - sensor_msgs::Imu latest_imu_msg_; - sensor_msgs::Imu last_used_imu_msg_; - nav_msgs::Odometry latest_odom_msg_; - nav_msgs::Odometry last_used_odom_msg_; + sensor_msgs::msg::Imu latest_imu_msg_; + sensor_msgs::msg::Imu last_used_imu_msg_; + nav_msgs::msg::Odometry latest_odom_msg_; + nav_msgs::msg::Odometry last_used_odom_msg_; - geometry_msgs::Twist latest_vel_msg_; + geometry_msgs::msg::Twist latest_vel_msg_; - std::vector a_cos_; - std::vector a_sin_; + std::vector a_cos_; + std::vector a_sin_; - sm_params input_; - sm_result output_; - LDP prev_ldp_scan_; + sm_params input_; + sm_result output_; + LDP prev_ldp_scan_; - // **** methods + // **** methods - void initParams(); - void processScan(LDP& curr_ldp_scan, const ros::Time& time); + void initParams(); + void processScan(LDP& curr_ldp_scan, const rclcpp::Time& time); - void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg, - LDP& ldp); - void PointCloudToLDP(const PointCloudT::ConstPtr& cloud, - LDP& ldp); + void laserScanToLDP(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan_msg, LDP& ldp); + void PointCloudToLDP(const std::shared_ptr& cloud, LDP& ldp); - void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg); - void cloudCallback (const PointCloudT::ConstPtr& cloud); + void scanCallback(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg); + void cloudCallback(const std::shared_ptr cloud); - void odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg); - void imuCallback (const sensor_msgs::Imu::ConstPtr& imu_msg); - void velCallback (const geometry_msgs::Twist::ConstPtr& twist_msg); - void velStmpCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg); + void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr odom_msg); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg); + void velCallback(const geometry_msgs::msg::Twist::ConstSharedPtr twist_msg); + void velStmpCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_msg); - void createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg); - bool getBaseToLaserTf (const std::string& frame_id); + void createCache(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg); + bool getBaseToLaserTf(const std::string& frame_id); - bool newKeyframeNeeded(const tf::Transform& d); + bool newKeyframeNeeded(const tf2::Transform& d); - void getPrediction(double& pr_ch_x, double& pr_ch_y, - double& pr_ch_a, double dt); + void getPrediction(double& pr_ch_x, double& pr_ch_y, double& pr_ch_a, double dt); - void createTfFromXYTheta(double x, double y, double theta, tf::Transform& t); + void createTfFromXYTheta(double x, double y, double theta, tf2::Transform& t); }; -} // namespace scan_tools +} // namespace scan_tools -#endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H +#endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H diff --git a/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher_nodelet.h b/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher_component.h similarity index 81% rename from laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher_nodelet.h rename to laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher_component.h index 24efdae..f07da49 100644 --- a/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher_nodelet.h +++ b/laser_scan_matcher/include/laser_scan_matcher/laser_scan_matcher_component.h @@ -35,25 +35,23 @@ * on Robotics and Automation (ICRA), 2008 */ -#ifndef LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_NODELET_H -#define LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_NODELET_H - -#include -#include +#ifndef LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_COMPONENT_H +#define LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_COMPONENT_H +#include #include -namespace scan_tools { - -class LaserScanMatcherNodelet : public nodelet::Nodelet +namespace scan_tools +{ +class LaserScanMatcherComponent : public rclcpp::Node { - public: - virtual void onInit (); +public: + explicit LaserScanMatcherComponent(const rclcpp::NodeOptions& options); - private: - boost::shared_ptr scan_matcher_; +private: + std::unique_ptr scan_matcher_; }; -} // namespace scan_tools +} // namespace scan_tools -#endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_NODELET_H +#endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_COMPONENT_H diff --git a/laser_scan_matcher/laser_scan_matcher_nodelet.xml b/laser_scan_matcher/laser_scan_matcher_nodelet.xml deleted file mode 100644 index 3fbb74f..0000000 --- a/laser_scan_matcher/laser_scan_matcher_nodelet.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - Laser Scan Matcher nodelet publisher. - - - diff --git a/laser_scan_matcher/package.xml b/laser_scan_matcher/package.xml index 3185590..c1cb82e 100644 --- a/laser_scan_matcher/package.xml +++ b/laser_scan_matcher/package.xml @@ -17,32 +17,32 @@ BSD LGPLv3 - catkin + ament_cmake csm geometry_msgs libpcl-all-dev nav_msgs - nodelet + rclcpp_components pcl_conversions pcl_ros - roscpp + rclcpp sensor_msgs - tf + tf2 csm geometry_msgs libpcl-all nav_msgs - nodelet + rclcpp_components pcl_ros pcl_conversions - roscpp + rclcpp sensor_msgs - tf + tf2 - + ament_cmake diff --git a/laser_scan_matcher/src/laser_scan_matcher.cpp b/laser_scan_matcher/src/laser_scan_matcher.cpp index 39a5698..dcb82f7 100644 --- a/laser_scan_matcher/src/laser_scan_matcher.cpp +++ b/laser_scan_matcher/src/laser_scan_matcher.cpp @@ -35,23 +35,30 @@ * on Robotics and Automation (ICRA), 2008 */ +#include #include #include -#include +#include +#include + +const static rclcpp::Logger LOGGER = rclcpp::get_logger("laser_scan_matcher"); namespace scan_tools { - -LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private): - nh_(nh), - nh_private_(nh_private), - initialized_(false), - received_imu_(false), - received_odom_(false), - received_vel_(false) +LaserScanMatcher::LaserScanMatcher(const rclcpp::Node::SharedPtr& node) + : node_(node) + , tf_buffer_(node_->get_clock()) + , tf_listener_(tf_buffer_) + , tf_broadcaster_(node_) + , initialized_(false) + , received_imu_(false) + , received_odom_(false) + , received_vel_(false) { - ROS_INFO("Starting LaserScanMatcher"); - + RCLCPP_INFO(LOGGER, "Starting LaserScanMatcher"); + tf_buffer_.setCreateTimerInterface( + std::make_shared(node_->get_node_base_interface(), node_->get_node_timers_interface())); + tf_buffer_.setUsingDedicatedThread(true); // **** init parameters initParams(); @@ -73,88 +80,92 @@ LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_privat if (publish_pose_) { - pose_publisher_ = nh_.advertise( - "pose2D", 5); + pose_publisher_ = node->create_publisher("pose2D", 5); } if (publish_pose_stamped_) { - pose_stamped_publisher_ = nh_.advertise( - "pose_stamped", 5); + pose_stamped_publisher_ = node->create_publisher("pose_stamped", 5); } if (publish_pose_with_covariance_) { - pose_with_covariance_publisher_ = nh_.advertise( - "pose_with_covariance", 5); + pose_with_covariance_publisher_ = + node->create_publisher("pose_with_covariance", 5); } if (publish_pose_with_covariance_stamped_) { - pose_with_covariance_stamped_publisher_ = nh_.advertise( - "pose_with_covariance_stamped", 5); + pose_with_covariance_stamped_publisher_ = + node->create_publisher("pose_with_covariance_stamped", 5); } // *** subscribers if (use_cloud_input_) { - cloud_subscriber_ = nh_.subscribe( - "cloud", 1, &LaserScanMatcher::cloudCallback, this); + cloud_subscriber_ = node_->create_subscription( + "cloud", 1, std::bind(&LaserScanMatcher::cloudCallback, this, std::placeholders::_1)); } else { - scan_subscriber_ = nh_.subscribe( - "scan", 1, &LaserScanMatcher::scanCallback, this); + scan_subscriber_ = node_->create_subscription( + "scan", 1, std::bind(&LaserScanMatcher::scanCallback, this, std::placeholders::_1)); } if (use_imu_) { - imu_subscriber_ = nh_.subscribe( - "imu/data", 1, &LaserScanMatcher::imuCallback, this); + imu_subscriber_ = node_->create_subscription( + "imu/data", 1, std::bind(&LaserScanMatcher::imuCallback, this, std::placeholders::_1)); } if (use_odom_) { - odom_subscriber_ = nh_.subscribe( - "odom", 1, &LaserScanMatcher::odomCallback, this); + odom_subscriber_ = node_->create_subscription( + "odom", 1, std::bind(&LaserScanMatcher::odomCallback, this, std::placeholders::_1)); } if (use_vel_) { if (stamped_vel_) - vel_subscriber_ = nh_.subscribe( - "vel", 1, &LaserScanMatcher::velStmpCallback, this); + vel_subscriber_ = node_->create_subscription( + "vel", 1, std::bind(&LaserScanMatcher::velStmpCallback, this, std::placeholders::_1)); else - vel_subscriber_ = nh_.subscribe( - "vel", 1, &LaserScanMatcher::velCallback, this); + vel_subscriber_ = node_->create_subscription( + "vel", 1, std::bind(&LaserScanMatcher::velCallback, this, std::placeholders::_1)); } } LaserScanMatcher::~LaserScanMatcher() { - ROS_INFO("Destroying LaserScanMatcher"); + RCLCPP_INFO(LOGGER, "Destroying LaserScanMatcher"); } void LaserScanMatcher::initParams() { - if (!nh_private_.getParam ("base_frame", base_frame_)) + node_->declare_parameter("base_frame"); + if (!node_->get_parameter("base_frame", base_frame_)) base_frame_ = "base_link"; - if (!nh_private_.getParam ("fixed_frame", fixed_frame_)) + node_->declare_parameter("fixed_frame"); + if (!node_->get_parameter("fixed_frame", fixed_frame_)) fixed_frame_ = "world"; // **** input type - laser scan, or point clouds? // if false, will subscribe to LaserScan msgs on /scan. // if true, will subscribe to PointCloud2 msgs on /cloud - if (!nh_private_.getParam ("use_cloud_input", use_cloud_input_)) - use_cloud_input_= false; + node_->declare_parameter("use_cloud_input"); + if (!node_->get_parameter("use_cloud_input", use_cloud_input_)) + use_cloud_input_ = false; if (use_cloud_input_) { - if (!nh_private_.getParam ("cloud_range_min", cloud_range_min_)) + node_->declare_parameter("cloud_range_min"); + if (node_->get_parameter("cloud_range_min", cloud_range_min_)) cloud_range_min_ = 0.1; - if (!nh_private_.getParam ("cloud_range_max", cloud_range_max_)) + node_->declare_parameter("cloud_range_max"); + if (node_->get_parameter("cloud_range_max", cloud_range_max_)) cloud_range_max_ = 50.0; - if (!nh_private_.getParam ("cloud_res", cloud_res_)) + node_->declare_parameter("cloud_res"); + if (node_->get_parameter("cloud_res", cloud_res_)) cloud_res_ = 0.05; input_.min_reading = cloud_range_min_; @@ -164,9 +175,11 @@ void LaserScanMatcher::initParams() // **** keyframe params: when to generate the keyframe scan // if either is set to 0, reduces to frame-to-frame matching - if (!nh_private_.getParam ("kf_dist_linear", kf_dist_linear_)) + node_->declare_parameter("kf_dist_linear"); + if (!node_->get_parameter("kf_dist_linear", kf_dist_linear_)) kf_dist_linear_ = 0.10; - if (!nh_private_.getParam ("kf_dist_angular", kf_dist_angular_)) + node_->declare_parameter("kf_dist_angular"); + if (!node_->get_parameter("kf_dist_angular", kf_dist_angular_)) kf_dist_angular_ = 10.0 * (M_PI / 180.0); kf_dist_linear_sq_ = kf_dist_linear_ * kf_dist_linear_; @@ -174,44 +187,55 @@ void LaserScanMatcher::initParams() // **** What predictions are available to speed up the ICP? // 1) imu - [theta] from imu yaw angle - /imu topic // 2) odom - [x, y, theta] from wheel odometry - /odom topic - // 3) vel - [x, y, theta] from velocity predictor - see alpha-beta predictors - /vel topic - // If more than one is enabled, priority is imu > odom > vel + // 3) vel - [x, y, theta] from velocity predictor - see alpha-beta predictors + // - /vel topic If more than one is enabled, priority is imu > odom > vel - if (!nh_private_.getParam ("use_imu", use_imu_)) + node_->declare_parameter("use_imu"); + if (!node_->get_parameter("use_imu", use_imu_)) use_imu_ = true; - if (!nh_private_.getParam ("use_odom", use_odom_)) + node_->declare_parameter("use_odom"); + if (!node_->get_parameter("use_odom", use_odom_)) use_odom_ = true; - if (!nh_private_.getParam ("use_vel", use_vel_)) + node_->declare_parameter("use_vel"); + if (!node_->get_parameter("use_vel", use_vel_)) use_vel_ = false; // **** Are velocity input messages stamped? // if false, will subscribe to Twist msgs on /vel // if true, will subscribe to TwistStamped msgs on /vel - if (!nh_private_.getParam ("stamped_vel", stamped_vel_)) + node_->declare_parameter("stamped_vel"); + if (!node_->get_parameter("stamped_vel", stamped_vel_)) stamped_vel_ = false; // **** How to publish the output? // tf (fixed_frame->base_frame), // pose message (pose of base frame in the fixed frame) - if (!nh_private_.getParam ("publish_tf", publish_tf_)) + node_->declare_parameter("publish_tf"); + if (!node_->get_parameter("publish_tf", publish_tf_)) publish_tf_ = true; - if (!nh_private_.getParam ("publish_pose", publish_pose_)) + node_->declare_parameter("publish_pose"); + if (!node_->get_parameter("publish_pose", publish_pose_)) publish_pose_ = true; - if (!nh_private_.getParam ("publish_pose_stamped", publish_pose_stamped_)) + node_->declare_parameter("publish_pose_stamped"); + if (!node_->get_parameter("publish_pose_stamped", publish_pose_stamped_)) publish_pose_stamped_ = false; - if (!nh_private_.getParam ("publish_pose_with_covariance", publish_pose_with_covariance_)) + node_->declare_parameter("publish_pose_with_covariance"); + if (!node_->get_parameter("publish_pose_with_covariance", publish_pose_with_covariance_)) publish_pose_with_covariance_ = false; - if (!nh_private_.getParam ("publish_pose_with_covariance_stamped", publish_pose_with_covariance_stamped_)) + node_->declare_parameter("publish_pose_with_covariance_stamped"); + if (!node_->get_parameter("publish_pose_with_covariance_stamped", publish_pose_with_covariance_stamped_)) publish_pose_with_covariance_stamped_ = false; - if (!nh_private_.getParam("position_covariance", position_covariance_)) + node_->declare_parameter("position_covariance"); + if (!node_->get_parameter("position_covariance", position_covariance_)) { position_covariance_.resize(3); std::fill(position_covariance_.begin(), position_covariance_.end(), 1e-9); } - if (!nh_private_.getParam("orientation_covariance", orientation_covariance_)) + node_->declare_parameter("orientation_covariance"); + if (!node_->get_parameter("orientation_covariance", orientation_covariance_)) { orientation_covariance_.resize(3); std::fill(orientation_covariance_.begin(), orientation_covariance_.end(), 1e-9); @@ -219,76 +243,94 @@ void LaserScanMatcher::initParams() // **** CSM parameters - comments copied from algos.h (by Andrea Censi) // Maximum angular displacement between scans - if (!nh_private_.getParam ("max_angular_correction_deg", input_.max_angular_correction_deg)) + node_->declare_parameter("max_angular_correction_deg"); + if (!node_->get_parameter("max_angular_correction_deg", input_.max_angular_correction_deg)) input_.max_angular_correction_deg = 45.0; // Maximum translation between scans (m) - if (!nh_private_.getParam ("max_linear_correction", input_.max_linear_correction)) + node_->declare_parameter("max_linear_correction"); + if (!node_->get_parameter("max_linear_correction", input_.max_linear_correction)) input_.max_linear_correction = 0.50; // Maximum ICP cycle iterations - if (!nh_private_.getParam ("max_iterations", input_.max_iterations)) + node_->declare_parameter("max_iterations"); + if (!node_->get_parameter("max_iterations", input_.max_iterations)) input_.max_iterations = 10; // A threshold for stopping (m) - if (!nh_private_.getParam ("epsilon_xy", input_.epsilon_xy)) + node_->declare_parameter("epsilon_xy"); + if (!node_->get_parameter("epsilon_xy", input_.epsilon_xy)) input_.epsilon_xy = 0.000001; // A threshold for stopping (rad) - if (!nh_private_.getParam ("epsilon_theta", input_.epsilon_theta)) + node_->declare_parameter("epsilon_theta"); + if (!node_->get_parameter("epsilon_theta", input_.epsilon_theta)) input_.epsilon_theta = 0.000001; // Maximum distance for a correspondence to be valid - if (!nh_private_.getParam ("max_correspondence_dist", input_.max_correspondence_dist)) + node_->declare_parameter("max_correspondence_dist"); + if (!node_->get_parameter("max_correspondence_dist", input_.max_correspondence_dist)) input_.max_correspondence_dist = 0.3; // Noise in the scan (m) - if (!nh_private_.getParam ("sigma", input_.sigma)) + node_->declare_parameter("sigma"); + if (!node_->get_parameter("sigma", input_.sigma)) input_.sigma = 0.010; // Use smart tricks for finding correspondences. - if (!nh_private_.getParam ("use_corr_tricks", input_.use_corr_tricks)) + node_->declare_parameter("use_corr_tricks"); + if (!node_->get_parameter("use_corr_tricks", input_.use_corr_tricks)) input_.use_corr_tricks = 1; // Restart: Restart if error is over threshold - if (!nh_private_.getParam ("restart", input_.restart)) + node_->declare_parameter("restart"); + if (!node_->get_parameter("restart", input_.restart)) input_.restart = 0; // Restart: Threshold for restarting - if (!nh_private_.getParam ("restart_threshold_mean_error", input_.restart_threshold_mean_error)) + node_->declare_parameter("restart_threshold_mean_error"); + if (!node_->get_parameter("restart_threshold_mean_error", input_.restart_threshold_mean_error)) input_.restart_threshold_mean_error = 0.01; // Restart: displacement for restarting. (m) - if (!nh_private_.getParam ("restart_dt", input_.restart_dt)) + node_->declare_parameter("restart_dt"); + if (!node_->get_parameter("restart_dt", input_.restart_dt)) input_.restart_dt = 1.0; // Restart: displacement for restarting. (rad) - if (!nh_private_.getParam ("restart_dtheta", input_.restart_dtheta)) + node_->declare_parameter("restart_dtheta"); + if (!node_->get_parameter("restart_dtheta", input_.restart_dtheta)) input_.restart_dtheta = 0.1; // Max distance for staying in the same clustering - if (!nh_private_.getParam ("clustering_threshold", input_.clustering_threshold)) + node_->declare_parameter("clustering_threshold"); + if (!node_->get_parameter("clustering_threshold", input_.clustering_threshold)) input_.clustering_threshold = 0.25; // Number of neighbour rays used to estimate the orientation - if (!nh_private_.getParam ("orientation_neighbourhood", input_.orientation_neighbourhood)) + node_->declare_parameter("orientation_neighbourhood"); + if (!node_->get_parameter("orientation_neighbourhood", input_.orientation_neighbourhood)) input_.orientation_neighbourhood = 20; // If 0, it's vanilla ICP - if (!nh_private_.getParam ("use_point_to_line_distance", input_.use_point_to_line_distance)) + node_->declare_parameter("use_point_to_line_distance"); + if (!node_->get_parameter("use_point_to_line_distance", input_.use_point_to_line_distance)) input_.use_point_to_line_distance = 1; // Discard correspondences based on the angles - if (!nh_private_.getParam ("do_alpha_test", input_.do_alpha_test)) + node_->declare_parameter("do_alpha_test"); + if (!node_->get_parameter("do_alpha_test", input_.do_alpha_test)) input_.do_alpha_test = 0; // Discard correspondences based on the angles - threshold angle, in degrees - if (!nh_private_.getParam ("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg)) + node_->declare_parameter("do_alpha_test_thresholdDeg"); + if (!node_->get_parameter("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg)) input_.do_alpha_test_thresholdDeg = 20.0; // Percentage of correspondences to consider: if 0.9, // always discard the top 10% of correspondences with more error - if (!nh_private_.getParam ("outliers_maxPerc", input_.outliers_maxPerc)) + node_->declare_parameter("outliers_maxPerc"); + if (!node_->get_parameter("outliers_maxPerc", input_.outliers_maxPerc)) input_.outliers_maxPerc = 0.90; // Parameters describing a simple adaptive algorithm for discarding. @@ -299,45 +341,56 @@ void LaserScanMatcher::initParams() // with the value of the error at the chosen percentile. // 4) Discard correspondences over the threshold. // This is useful to be conservative; yet remove the biggest errors. - if (!nh_private_.getParam ("outliers_adaptive_order", input_.outliers_adaptive_order)) + node_->declare_parameter("outliers_adaptive_order"); + if (!node_->get_parameter("outliers_adaptive_order", input_.outliers_adaptive_order)) input_.outliers_adaptive_order = 0.7; - if (!nh_private_.getParam ("outliers_adaptive_mult", input_.outliers_adaptive_mult)) + node_->declare_parameter("outliers_adaptive_mult"); + if (!node_->get_parameter("outliers_adaptive_mult", input_.outliers_adaptive_mult)) input_.outliers_adaptive_mult = 2.0; - // If you already have a guess of the solution, you can compute the polar angle - // of the points of one scan in the new position. If the polar angle is not a monotone - // function of the readings index, it means that the surface is not visible in the - // next position. If it is not visible, then we don't use it for matching. - if (!nh_private_.getParam ("do_visibility_test", input_.do_visibility_test)) + // If you already have a guess of the solution, you can compute the polar + // angle of the points of one scan in the new position. If the polar angle is + // not a monotone function of the readings index, it means that the surface is + // not visible in the next position. If it is not visible, then we don't use + // it for matching. + node_->declare_parameter("do_visibility_test"); + if (!node_->get_parameter("do_visibility_test", input_.do_visibility_test)) input_.do_visibility_test = 0; // no two points in laser_sens can have the same corr. - if (!nh_private_.getParam ("outliers_remove_doubles", input_.outliers_remove_doubles)) + node_->declare_parameter("outliers_remove_doubles"); + if (!node_->get_parameter("outliers_remove_doubles", input_.outliers_remove_doubles)) input_.outliers_remove_doubles = 1; - // If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov - if (!nh_private_.getParam ("do_compute_covariance", input_.do_compute_covariance)) + // If 1, computes the covariance of ICP using the method + // http://purl.org/censi/2006/icpcov + node_->declare_parameter("do_compute_covariance"); + if (!node_->get_parameter("do_compute_covariance", input_.do_compute_covariance)) input_.do_compute_covariance = 0; // Checks that find_correspondences_tricks gives the right answer - if (!nh_private_.getParam ("debug_verify_tricks", input_.debug_verify_tricks)) + node_->declare_parameter("debug_verify_tricks"); + if (!node_->get_parameter("debug_verify_tricks", input_.debug_verify_tricks)) input_.debug_verify_tricks = 0; - // If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the - // incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence."); - if (!nh_private_.getParam ("use_ml_weights", input_.use_ml_weights)) + // If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to + // compute the incidence beta, and the factor (1/cos^2(beta)) used to weight + // the correspondence."); + node_->declare_parameter("use_ml_weights"); + if (!node_->get_parameter("use_ml_weights", input_.use_ml_weights)) input_.use_ml_weights = 0; // If 1, the field 'readings_sigma' in the second scan is used to weight the // correspondence by 1/sigma^2 - if (!nh_private_.getParam ("use_sigma_weights", input_.use_sigma_weights)) + node_->declare_parameter("use_sigma_weights"); + if (!node_->get_parameter("use_sigma_weights", input_.use_sigma_weights)) input_.use_sigma_weights = 0; } -void LaserScanMatcher::imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) +void LaserScanMatcher::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg) { - boost::mutex::scoped_lock(mutex_); + std::unique_lock _(mutex_); latest_imu_msg_ = *imu_msg; if (!received_imu_) { @@ -346,9 +399,9 @@ void LaserScanMatcher::imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) } } -void LaserScanMatcher::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg) +void LaserScanMatcher::odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr odom_msg) { - boost::mutex::scoped_lock(mutex_); + std::unique_lock _(mutex_); latest_odom_msg_ = *odom_msg; if (!received_odom_) { @@ -357,34 +410,34 @@ void LaserScanMatcher::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg } } -void LaserScanMatcher::velCallback(const geometry_msgs::Twist::ConstPtr& twist_msg) +void LaserScanMatcher::velCallback(const geometry_msgs::msg::Twist::ConstSharedPtr twist_msg) { - boost::mutex::scoped_lock(mutex_); + std::unique_lock _(mutex_); latest_vel_msg_ = *twist_msg; received_vel_ = true; } -void LaserScanMatcher::velStmpCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg) +void LaserScanMatcher::velStmpCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_msg) { - boost::mutex::scoped_lock(mutex_); + std::unique_lock _(mutex_); latest_vel_msg_ = twist_msg->twist; received_vel_ = true; } -void LaserScanMatcher::cloudCallback (const PointCloudT::ConstPtr& cloud) +void LaserScanMatcher::cloudCallback(const std::shared_ptr cloud) { // **** if first scan, cache the tf from base to the scanner - std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud->header); + std_msgs::msg::Header cloud_header = pcl_conversions::fromPCL(cloud->header); if (!initialized_) { // cache the static tf from base to laser if (!getBaseToLaserTf(cloud_header.frame_id)) { - ROS_WARN("Skipping scan"); + RCLCPP_WARN(LOGGER, "Skipping scan"); return; } @@ -398,18 +451,18 @@ void LaserScanMatcher::cloudCallback (const PointCloudT::ConstPtr& cloud) processScan(curr_ldp_scan, cloud_header.stamp); } -void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg) +void LaserScanMatcher::scanCallback(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg) { // **** if first scan, cache the tf from base to the scanner if (!initialized_) { - createCache(scan_msg); // caches the sin and cos of all angles + createCache(scan_msg); // caches the sin and cos of all angles // cache the static tf from base to laser if (!getBaseToLaserTf(scan_msg->header.frame_id)) { - ROS_WARN("Skipping scan"); + RCLCPP_WARN(LOGGER, "Skipping scan"); return; } @@ -423,9 +476,10 @@ void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& sca processScan(curr_ldp_scan, scan_msg->header.stamp); } -void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) +void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const rclcpp::Time& time) { - ros::WallTime start = ros::WallTime::now(); + rclcpp::Clock clock(RCL_STEADY_TIME); + rclcpp::Time start = clock.now(); // CSM is used in the following way: // The scans are always in the laser frame @@ -446,18 +500,18 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) prev_ldp_scan_->true_pose[1] = 0.0; prev_ldp_scan_->true_pose[2] = 0.0; - input_.laser_ref = prev_ldp_scan_; + input_.laser_ref = prev_ldp_scan_; input_.laser_sens = curr_ldp_scan; // **** estimated change since last scan - double dt = (time - last_icp_time_).toSec(); + double dt = (time - last_icp_time_).seconds(); double pr_ch_x, pr_ch_y, pr_ch_a; getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt); // the predicted change of the laser's position, in the fixed frame - tf::Transform pr_ch; + tf2::Transform pr_ch; createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch); // account for the change since the last kf, in the fixed frame @@ -466,12 +520,12 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) // the predicted change of the laser's position, in the laser frame - tf::Transform pr_ch_l; - pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ; + tf2::Transform pr_ch_l; + pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_; input_.first_guess[0] = pr_ch_l.getOrigin().getX(); input_.first_guess[1] = pr_ch_l.getOrigin().getY(); - input_.first_guess[2] = tf::getYaw(pr_ch_l.getRotation()); + input_.first_guess[2] = tf2::getYaw(pr_ch_l.getRotation()); // If they are non-Null, free covariance gsl matrices to avoid leaking memory if (output_.cov_x_m) @@ -493,13 +547,12 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) // *** scan match - using point to line icp from CSM sm_icp(&input_, &output_); - tf::Transform corr_ch; + tf2::Transform corr_ch; if (output_.valid) { - // the correction of the laser's position, in the laser frame - tf::Transform corr_ch_l; + tf2::Transform corr_ch_l; createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l); // the correction of the base's position, in the base frame @@ -513,101 +566,94 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) if (publish_pose_) { // unstamped Pose2D message - geometry_msgs::Pose2D::Ptr pose_msg; - pose_msg = boost::make_shared(); + auto pose_msg = std::make_unique(); pose_msg->x = f2b_.getOrigin().getX(); pose_msg->y = f2b_.getOrigin().getY(); - pose_msg->theta = tf::getYaw(f2b_.getRotation()); - pose_publisher_.publish(pose_msg); + pose_msg->theta = tf2::getYaw(f2b_.getRotation()); + pose_publisher_->publish(std::move(pose_msg)); } if (publish_pose_stamped_) { // stamped Pose message - geometry_msgs::PoseStamped::Ptr pose_stamped_msg; - pose_stamped_msg = boost::make_shared(); + auto pose_stamped_msg = std::make_unique(); - pose_stamped_msg->header.stamp = time; + pose_stamped_msg->header.stamp = time; pose_stamped_msg->header.frame_id = fixed_frame_; - tf::poseTFToMsg(f2b_, pose_stamped_msg->pose); + tf2::toMsg(f2b_, pose_stamped_msg->pose); - pose_stamped_publisher_.publish(pose_stamped_msg); + pose_stamped_publisher_->publish(std::move(pose_stamped_msg)); } if (publish_pose_with_covariance_) { // unstamped PoseWithCovariance message - geometry_msgs::PoseWithCovariance::Ptr pose_with_covariance_msg; - pose_with_covariance_msg = boost::make_shared(); - tf::poseTFToMsg(f2b_, pose_with_covariance_msg->pose); + auto pose_with_covariance_msg = std::make_unique(); + tf2::toMsg(f2b_, pose_with_covariance_msg->pose); if (input_.do_compute_covariance) { - pose_with_covariance_msg->covariance = boost::assign::list_of - (gsl_matrix_get(output_.cov_x_m, 0, 0)) (0) (0) (0) (0) (0) - (0) (gsl_matrix_get(output_.cov_x_m, 0, 1)) (0) (0) (0) (0) - (0) (0) (static_cast(position_covariance_[2])) (0) (0) (0) - (0) (0) (0) (static_cast(orientation_covariance_[0])) (0) (0) - (0) (0) (0) (0) (static_cast(orientation_covariance_[1])) (0) - (0) (0) (0) (0) (0) (gsl_matrix_get(output_.cov_x_m, 0, 2)); + pose_with_covariance_msg->covariance = boost::assign::list_of(gsl_matrix_get(output_.cov_x_m, 0, 0))(0)(0)(0)( + 0)(0)(0)(gsl_matrix_get(output_.cov_x_m, 0, 1))(0)(0)(0)(0)(0)(0)(static_cast( + position_covariance_[2]))(0)(0)(0)(0)(0)(0)(static_cast(orientation_covariance_[0]))(0)(0)(0)(0)(0)( + 0)(static_cast(orientation_covariance_[1]))(0)(0)(0)(0)(0)(0)(gsl_matrix_get(output_.cov_x_m, 0, 2)); } else { - pose_with_covariance_msg->covariance = boost::assign::list_of - (static_cast(position_covariance_[0])) (0) (0) (0) (0) (0) - (0) (static_cast(position_covariance_[1])) (0) (0) (0) (0) - (0) (0) (static_cast(position_covariance_[2])) (0) (0) (0) - (0) (0) (0) (static_cast(orientation_covariance_[0])) (0) (0) - (0) (0) (0) (0) (static_cast(orientation_covariance_[1])) (0) - (0) (0) (0) (0) (0) (static_cast(orientation_covariance_[2])); + pose_with_covariance_msg->covariance = + boost::assign::list_of(static_cast(position_covariance_[0]))(0)(0)(0)(0)(0)(0)(static_cast( + position_covariance_[1]))(0)(0)(0)(0)(0)(0)(static_cast(position_covariance_[2]))(0)(0)(0)(0)( + 0)(0)(static_cast(orientation_covariance_[0]))(0)(0)(0)(0)(0)(0)(static_cast( + orientation_covariance_[1]))(0)(0)(0)(0)(0)(0)(static_cast(orientation_covariance_[2])); } - pose_with_covariance_publisher_.publish(pose_with_covariance_msg); + // TODO(JafarAbdi): std::variant .? + // pose_with_covariance_publisher_->publish( + // std::move(pose_with_covariance_msg)); } if (publish_pose_with_covariance_stamped_) { // stamped Pose message - geometry_msgs::PoseWithCovarianceStamped::Ptr pose_with_covariance_stamped_msg; - pose_with_covariance_stamped_msg = boost::make_shared(); + auto pose_with_covariance_stamped_msg = std::make_unique(); - pose_with_covariance_stamped_msg->header.stamp = time; + pose_with_covariance_stamped_msg->header.stamp = time; pose_with_covariance_stamped_msg->header.frame_id = fixed_frame_; - tf::poseTFToMsg(f2b_, pose_with_covariance_stamped_msg->pose.pose); + tf2::toMsg(f2b_, pose_with_covariance_stamped_msg->pose.pose); if (input_.do_compute_covariance) { - pose_with_covariance_stamped_msg->pose.covariance = boost::assign::list_of - (gsl_matrix_get(output_.cov_x_m, 0, 0)) (0) (0) (0) (0) (0) - (0) (gsl_matrix_get(output_.cov_x_m, 0, 1)) (0) (0) (0) (0) - (0) (0) (static_cast(position_covariance_[2])) (0) (0) (0) - (0) (0) (0) (static_cast(orientation_covariance_[0])) (0) (0) - (0) (0) (0) (0) (static_cast(orientation_covariance_[1])) (0) - (0) (0) (0) (0) (0) (gsl_matrix_get(output_.cov_x_m, 0, 2)); + pose_with_covariance_stamped_msg->pose.covariance = boost::assign::list_of( + gsl_matrix_get(output_.cov_x_m, 0, 0))(0)(0)(0)(0)(0)(0)(gsl_matrix_get(output_.cov_x_m, 0, 1))(0)(0)(0)(0)( + 0)(0)(static_cast(position_covariance_[2]))(0)(0)(0)(0)(0)(0)( + static_cast(orientation_covariance_[0]))(0)(0)(0)(0)(0)(0)( + static_cast(orientation_covariance_[1]))(0)(0)(0)(0)(0)(0)(gsl_matrix_get(output_.cov_x_m, 0, 2)); } else { - pose_with_covariance_stamped_msg->pose.covariance = boost::assign::list_of - (static_cast(position_covariance_[0])) (0) (0) (0) (0) (0) - (0) (static_cast(position_covariance_[1])) (0) (0) (0) (0) - (0) (0) (static_cast(position_covariance_[2])) (0) (0) (0) - (0) (0) (0) (static_cast(orientation_covariance_[0])) (0) (0) - (0) (0) (0) (0) (static_cast(orientation_covariance_[1])) (0) - (0) (0) (0) (0) (0) (static_cast(orientation_covariance_[2])); + pose_with_covariance_stamped_msg->pose.covariance = + boost::assign::list_of(static_cast(position_covariance_[0]))(0)(0)(0)(0)(0)(0)(static_cast( + position_covariance_[1]))(0)(0)(0)(0)(0)(0)(static_cast(position_covariance_[2]))(0)(0)(0)(0)( + 0)(0)(static_cast(orientation_covariance_[0]))(0)(0)(0)(0)(0)(0)(static_cast( + orientation_covariance_[1]))(0)(0)(0)(0)(0)(0)(static_cast(orientation_covariance_[2])); } - pose_with_covariance_stamped_publisher_.publish(pose_with_covariance_stamped_msg); + pose_with_covariance_stamped_publisher_->publish(std::move(pose_with_covariance_stamped_msg)); } if (publish_tf_) { - tf::StampedTransform transform_msg (f2b_, time, fixed_frame_, base_frame_); - tf_broadcaster_.sendTransform (transform_msg); + geometry_msgs::msg::TransformStamped transform_msg; + transform_msg.transform = tf2::toMsg(f2b_); + transform_msg.header.stamp = time; + transform_msg.header.frame_id = fixed_frame_; + transform_msg.child_frame_id = base_frame_; + tf_broadcaster_.sendTransform(transform_msg); } } else { corr_ch.setIdentity(); - ROS_WARN("Error in scan matching"); + RCLCPP_WARN(LOGGER, "Error in scan matching"); } // **** swap old and new @@ -628,23 +674,24 @@ void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) // **** statistics - double dur = (ros::WallTime::now() - start).toSec() * 1e3; - ROS_DEBUG("Scan matcher total duration: %.1f ms", dur); + double dur = (clock.now() - start).seconds() * 1e3; + RCLCPP_DEBUG(LOGGER, "Scan matcher total duration: %.1f ms", dur); } -bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d) +bool LaserScanMatcher::newKeyframeNeeded(const tf2::Transform& d) { - if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_) return true; + if (fabs(tf2::getYaw(d.getRotation())) > kf_dist_angular_) + return true; double x = d.getOrigin().getX(); double y = d.getOrigin().getY(); - if (x*x + y*y > kf_dist_linear_sq_) return true; + if (x * x + y * y > kf_dist_linear_sq_) + return true; return false; } -void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud, - LDP& ldp) +void LaserScanMatcher::PointCloudToLDP(const std::shared_ptr& cloud, LDP& ldp) { double max_d2 = cloud_res_ * cloud_res_; @@ -659,7 +706,7 @@ void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud, double dx = pa.x - pb.x; double dy = pa.y - pb.y; - double d2 = dx*dx + dy*dy; + double d2 = dx * dx + dy * dy; if (d2 > max_d2) { @@ -676,13 +723,12 @@ void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud, // calculate position in laser frame if (is_nan(cloud_f.points[i].x) || is_nan(cloud_f.points[i].y)) { - ROS_WARN("Laser Scan Matcher: Cloud input contains NaN values. \ + RCLCPP_WARN(LOGGER, "Laser Scan Matcher: Cloud input contains NaN values. \ Please use a filtered cloud input."); } else { - double r = sqrt(cloud_f.points[i].x * cloud_f.points[i].x + - cloud_f.points[i].y * cloud_f.points[i].y); + double r = sqrt(cloud_f.points[i].x * cloud_f.points[i].x + cloud_f.points[i].y * cloud_f.points[i].y); if (r > cloud_range_min_ && r < cloud_range_max_) { @@ -697,11 +743,11 @@ void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud, } ldp->theta[i] = atan2(cloud_f.points[i].y, cloud_f.points[i].x); - ldp->cluster[i] = -1; + ldp->cluster[i] = -1; } ldp->min_theta = ldp->theta[0]; - ldp->max_theta = ldp->theta[n-1]; + ldp->max_theta = ldp->theta[n - 1]; ldp->odometry[0] = 0.0; ldp->odometry[1] = 0.0; @@ -712,8 +758,7 @@ void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud, ldp->true_pose[2] = 0.0; } -void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg, - LDP& ldp) +void LaserScanMatcher::laserScanToLDP(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan_msg, LDP& ldp) { unsigned int n = scan_msg->ranges.size(); ldp = ld_alloc_new(n); @@ -737,13 +782,13 @@ void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& sc ldp->readings[i] = -1; // for invalid range } - ldp->theta[i] = scan_msg->angle_min + i * scan_msg->angle_increment; + ldp->theta[i] = scan_msg->angle_min + i * scan_msg->angle_increment; - ldp->cluster[i] = -1; + ldp->cluster[i] = -1; } ldp->min_theta = ldp->theta[0]; - ldp->max_theta = ldp->theta[n-1]; + ldp->max_theta = ldp->theta[n - 1]; ldp->odometry[0] = 0.0; ldp->odometry[1] = 0.0; @@ -754,7 +799,7 @@ void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& sc ldp->true_pose[2] = 0.0; } -void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg) +void LaserScanMatcher::createCache(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg) { a_cos_.clear(); a_sin_.clear(); @@ -770,24 +815,31 @@ void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan input_.max_reading = scan_msg->range_max; } -bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id) +bool LaserScanMatcher::getBaseToLaserTf(const std::string& frame_id) { - ros::Time t = ros::Time::now(); - - tf::StampedTransform base_to_laser_tf; - try - { - tf_listener_.waitForTransform( - base_frame_, frame_id, t, ros::Duration(1.0)); - tf_listener_.lookupTransform ( - base_frame_, frame_id, t, base_to_laser_tf); - } - catch (tf::TransformException ex) - { - ROS_WARN("Could not get initial transform from base to laser frame, %s", ex.what()); + rclcpp::Time t = node_->now(); + + geometry_msgs::msg::TransformStamped base_to_laser_tf; + + bool callback_timeout = false; + auto transform_future = tf_buffer_.waitForTransform( + base_frame_, frame_id, t, rclcpp::Duration(1.0), + [&callback_timeout, &base_to_laser_tf](const tf2_ros::TransformStampedFuture& future) { + // May throw due to timeout + try + { + base_to_laser_tf = future.get(); + } + catch (...) + { + callback_timeout = true; + } + }); + // Wait until we get the transform or we timeout + transform_future.wait(); + if (callback_timeout) return false; - } - base_to_laser_ = base_to_laser_tf; + tf2::fromMsg(base_to_laser_tf.transform, base_to_laser_); laser_to_base_ = base_to_laser_.inverse(); return true; @@ -795,10 +847,9 @@ bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id) // returns the predicted change in pose (in fixed frame) // since the last time we did icp -void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y, - double& pr_ch_a, double dt) +void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y, double& pr_ch_a, double dt) { - boost::mutex::scoped_lock(mutex_); + std::unique_lock _(mutex_); // **** base case - no input available, use zero-motion model pr_ch_x = 0.0; @@ -812,24 +863,26 @@ void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y, pr_ch_y = dt * latest_vel_msg_.linear.y; pr_ch_a = dt * latest_vel_msg_.angular.z; - if (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI; - else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI; + if (pr_ch_a >= M_PI) + pr_ch_a -= 2.0 * M_PI; + else if (pr_ch_a < -M_PI) + pr_ch_a += 2.0 * M_PI; } // **** use wheel odometry if (use_odom_ && received_odom_) { - pr_ch_x = latest_odom_msg_.pose.pose.position.x - - last_used_odom_msg_.pose.pose.position.x; + pr_ch_x = latest_odom_msg_.pose.pose.position.x - last_used_odom_msg_.pose.pose.position.x; - pr_ch_y = latest_odom_msg_.pose.pose.position.y - - last_used_odom_msg_.pose.pose.position.y; + pr_ch_y = latest_odom_msg_.pose.pose.position.y - last_used_odom_msg_.pose.pose.position.y; - pr_ch_a = tf::getYaw(latest_odom_msg_.pose.pose.orientation) - - tf::getYaw(last_used_odom_msg_.pose.pose.orientation); + pr_ch_a = + tf2::getYaw(latest_odom_msg_.pose.pose.orientation) - tf2::getYaw(last_used_odom_msg_.pose.pose.orientation); - if (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI; - else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI; + if (pr_ch_a >= M_PI) + pr_ch_a -= 2.0 * M_PI; + else if (pr_ch_a < -M_PI) + pr_ch_a += 2.0 * M_PI; last_used_odom_msg_ = latest_odom_msg_; } @@ -837,23 +890,23 @@ void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y, // **** use imu if (use_imu_ && received_imu_) { - pr_ch_a = tf::getYaw(latest_imu_msg_.orientation) - - tf::getYaw(last_used_imu_msg_.orientation); + pr_ch_a = tf2::getYaw(latest_imu_msg_.orientation) - tf2::getYaw(last_used_imu_msg_.orientation); - if (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI; - else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI; + if (pr_ch_a >= M_PI) + pr_ch_a -= 2.0 * M_PI; + else if (pr_ch_a < -M_PI) + pr_ch_a += 2.0 * M_PI; last_used_imu_msg_ = latest_imu_msg_; } } -void LaserScanMatcher::createTfFromXYTheta( - double x, double y, double theta, tf::Transform& t) +void LaserScanMatcher::createTfFromXYTheta(double x, double y, double theta, tf2::Transform& t) { - t.setOrigin(tf::Vector3(x, y, 0.0)); - tf::Quaternion q; + t.setOrigin(tf2::Vector3(x, y, 0.0)); + tf2::Quaternion q; q.setRPY(0.0, 0.0, theta); t.setRotation(q); } -} // namespace scan_tools +} // namespace scan_tools diff --git a/laser_scan_matcher/src/laser_scan_matcher_nodelet.cpp b/laser_scan_matcher/src/laser_scan_matcher_component.cpp similarity index 76% rename from laser_scan_matcher/src/laser_scan_matcher_nodelet.cpp rename to laser_scan_matcher/src/laser_scan_matcher_component.cpp index 9fc4684..0b22a6c 100644 --- a/laser_scan_matcher/src/laser_scan_matcher_nodelet.cpp +++ b/laser_scan_matcher/src/laser_scan_matcher_component.cpp @@ -35,21 +35,17 @@ * on Robotics and Automation (ICRA), 2008 */ -#include +#include -typedef scan_tools::LaserScanMatcherNodelet LaserScanMatcherNodelet; +typedef scan_tools::LaserScanMatcherComponent LaserScanMatcherComponent; -PLUGINLIB_EXPORT_CLASS(LaserScanMatcherNodelet, nodelet::Nodelet) - -void LaserScanMatcherNodelet::onInit() +LaserScanMatcherComponent::LaserScanMatcherComponent(const rclcpp::NodeOptions& options) + : Node("laser_scan_matcher", options) { - NODELET_INFO("Initializing LaserScanMatcher Nodelet"); - - // TODO: Do we want the single threaded or multithreaded NH? - ros::NodeHandle nh = getMTNodeHandle(); - ros::NodeHandle nh_private = getMTPrivateNodeHandle(); - - scan_matcher_ = boost::shared_ptr( - new LaserScanMatcher(nh, nh_private)); + RCLCPP_INFO(get_logger(), "Initializing LaserScanMatcher component"); + scan_matcher_ = std::make_unique(shared_from_this()); } +// Register the component with class_loader +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(LaserScanMatcherComponent) \ No newline at end of file diff --git a/laser_scan_matcher/src/laser_scan_matcher_node.cpp b/laser_scan_matcher/src/laser_scan_matcher_node.cpp index 58d1b6a..7bc0767 100644 --- a/laser_scan_matcher/src/laser_scan_matcher_node.cpp +++ b/laser_scan_matcher/src/laser_scan_matcher_node.cpp @@ -39,10 +39,9 @@ int main(int argc, char** argv) { - ros::init(argc, argv, "LaserScanMatcher"); - ros::NodeHandle nh; - ros::NodeHandle nh_private("~"); - scan_tools::LaserScanMatcher laser_scan_matcher(nh, nh_private); - ros::spin(); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("laser_scan_matcher_node"); + scan_tools::LaserScanMatcher laser_scan_matcher(node); + rclcpp::spin(node); return 0; } diff --git a/laser_scan_sparsifier/COLCON_IGNORE b/laser_scan_sparsifier/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/laser_scan_splitter/COLCON_IGNORE b/laser_scan_splitter/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/ncd_parser/COLCON_IGNORE b/ncd_parser/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/polar_scan_matcher/COLCON_IGNORE b/polar_scan_matcher/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/scan_to_cloud_converter/COLCON_IGNORE b/scan_to_cloud_converter/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/scan_tools/COLCON_IGNORE b/scan_tools/COLCON_IGNORE new file mode 100644 index 0000000..e69de29