basic working rnav demo

mrpt_pointcloud_pipeline/CMakeLists.txt

@@ -76,7 +76,9 @@ install(TARGETS ${PROJECT_NAME}_node
 )
 
 install(
-  DIRECTORY launch
+  DIRECTORY
+    launch
+    params
   DESTINATION share/${PROJECT_NAME}
 )
 

mrpt_pointcloud_pipeline/launch/demo_with_mvsim.launch.py

@@ -6,9 +6,9 @@
 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'
@@ -27,7 +27,8 @@ def generate_launch_description():
     )
     filter_yaml_file_arg = DeclareLaunchArgument(
         'filter_yaml_file',
-        default_value= os.path.join(os.path.dirname(__file__), 'local-obstacles-decimation-filter.yaml')
+        default_value=os.path.join(os.path.dirname(
+            __file__), 'local-obstacles-decimation-filter.yaml')
     )
     filter_output_layer_name_arg = DeclareLaunchArgument(
         'filter_output_layer_name',
@@ -54,12 +55,15 @@ def generate_launch_description():
         output='screen',
         parameters=[
             {'source_topics_2dscan': LaunchConfiguration('lidar_topic_name')},
-            {'source_topics_pointclouds': LaunchConfiguration('points_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')},
+            {'filter_output_layer_name': LaunchConfiguration(
+                'filter_output_layer_name')},
             {'time_window': LaunchConfiguration('time_window')},
-            {'topic_local_map_pointcloud': LaunchConfiguration('filter_output_topic_name')},
+            {'topic_local_map_pointcloud': LaunchConfiguration(
+                'filter_output_topic_name')},
             {'frameid_reference': LaunchConfiguration('frameid_reference')},
             {'frameid_robot': LaunchConfiguration('frameid_robot')},
         ],
@@ -68,16 +72,18 @@ def generate_launch_description():
     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)
+    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!")
+        raise Exception(
+            f"Launch file '{mvsim_launch_file_path}' does not exist!")
 
     return LaunchDescription([
         IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(mvsim_launch_file_path)
-            ),
+            PythonLaunchDescriptionSource(mvsim_launch_file_path)
+        ),
         lidar_topic_name_arg,
         points_topic_name_arg,
         show_gui_arg,

mrpt_reactivenav2d/CMakeLists.txt

@@ -96,7 +96,7 @@ install(TARGETS ${PROJECT_NAME}_node
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 install(DIRECTORY
   launch
-  tutorial
+  params
   DESTINATION share/${PROJECT_NAME}
 )
 

mrpt_reactivenav2d/include/mrpt_reactivenav2d/mrpt_reactivenav2d_node.hpp

@@ -90,16 +90,16 @@ class ReactiveNav2DNode : public rclcpp::Node
 
 	CTimeLogger m_profiler;
 	bool m_1st_time_init;  //!< Reactive initialization done?
-	double m_target_allowed_distance;
-	double m_nav_period;
+	double m_target_allowed_distance = 0.40;
+	double m_nav_period = 0.10;
 
 	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_robot_shape;
 	std::string m_sub_topic_wp_seq = "reactive_nav_waypoints";
-	std::string m_sub_topic_odometry = "odom";
+	std::string m_sub_topic_odometry = "/odom";
 
-	std::string m_pub_topic_cmd_vel = "cmd_vel";
+	std::string m_pub_topic_cmd_vel = "/cmd_vel";
 
 	std::string m_frameid_reference = "map";
 	std::string m_frameid_robot = "base_link";

mrpt_reactivenav2d/tutorial/reactive2d_config.ini → mrpt_reactivenav2d/params/reactive2d_default.ini

@@ -51,7 +51,7 @@ min_dist                                          = 0.100000
 angle_tolerance                                   = 5.000000
 too_old_seconds                                   = 1.000000
 previous_keyframes                                = 1                    // (Default: 1) How many previous keyframes will be compared with the latest pointcloud.
-max_deletion_ratio                                = 0.400000             // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it'd be too suspicious and may indicate a failure of this filter.
+max_deletion_ratio                                = 0.400000             // (Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it would be too suspicious and may indicate a failure of this filter.
 
 
 [CHolonomicFullEval]

mrpt_reactivenav2d/src/mrpt_reactivenav2d_node.cpp

@@ -185,62 +185,78 @@ ReactiveNav2DNode::ReactiveNav2DNode(const rclcpp::NodeOptions& options)
 
 void ReactiveNav2DNode::read_parameters()
 {
-	this->declare_parameter<std::string>(
+	declare_parameter<std::string>(
 		"cfg_file_reactive", "reactive2d_config.ini");
-	this->get_parameter("cfg_file_reactive", m_cfg_file_reactive);
+	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<double>("target_allowed_distance", 0.40);
-	this->get_parameter("target_allowed_distance", m_target_allowed_distance);
+	declare_parameter<double>(
+		"target_allowed_distance", m_target_allowed_distance);
+	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<double>("nav_period", 0.10);
-	this->get_parameter("nav_period", m_nav_period);
+	declare_parameter<double>("nav_period", m_nav_period);
+	get_parameter("nav_period", m_nav_period);
 	RCLCPP_INFO(this->get_logger(), "nav_period: %f", m_nav_period);
 
-	this->declare_parameter<std::string>("frameid_reference", "odom");
-	this->get_parameter("frameid_reference", m_frameid_reference);
+	declare_parameter<std::string>("frameid_reference", m_frameid_reference);
+	get_parameter("frameid_reference", m_frameid_reference);
 	RCLCPP_INFO(
 		this->get_logger(), "frameid_reference: %s",
 		m_frameid_reference.c_str());
 
-	this->declare_parameter<std::string>("frameid_robot", "base_link");
-	this->get_parameter("frameid_robot", m_frameid_robot);
+	declare_parameter<std::string>("frameid_robot", m_frameid_robot);
+	get_parameter("frameid_robot", m_frameid_robot);
 	RCLCPP_INFO(
 		this->get_logger(), "frameid_robot: %s", m_frameid_robot.c_str());
 
-	this->declare_parameter<std::string>("topic_wp_seq", "waypointsequence");
-	this->get_parameter("topic_wp_seq", m_sub_topic_wp_seq);
+	declare_parameter<std::string>("topic_wp_seq", m_sub_topic_wp_seq);
+	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<std::string>("topic_odometry", "/odometry");
-	this->get_parameter("topic_odometry", m_sub_topic_odometry);
+	declare_parameter<std::string>(
+		"topic_reactive_nav_goal", m_sub_topic_reactive_nav_goal);
+	get_parameter("topic_reactive_nav_goal", m_sub_topic_reactive_nav_goal);
+	RCLCPP_INFO(
+		this->get_logger(), "topic_reactive_nav_goal: %s",
+		m_sub_topic_reactive_nav_goal.c_str());
+
+	declare_parameter<std::string>("topic_odometry", m_sub_topic_odometry);
+	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<std::string>("topic_cmd_vel", "/cmd_vel");
-	this->get_parameter("topic_cmd_vel", m_pub_topic_cmd_vel);
+	declare_parameter<std::string>("topic_cmd_vel", m_pub_topic_cmd_vel);
+	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<std::string>("topic_obstacles", "/pointcloud");
-	this->get_parameter("topic_obstacles", m_sub_topic_local_obstacles);
+	declare_parameter<std::string>(
+		"topic_obstacles", m_sub_topic_local_obstacles);
+	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<bool>("save_nav_log", false);
-	this->get_parameter("save_nav_log", m_save_nav_log);
+	declare_parameter<std::string>(
+		"topic_robot_shape", m_sub_topic_robot_shape);
+	get_parameter("topic_robot_shape", m_sub_topic_robot_shape);
+	RCLCPP_INFO(
+		this->get_logger(), "topic_robot_shape: %s",
+		m_sub_topic_robot_shape.c_str());
+
+	declare_parameter<bool>("save_nav_log", false);
+	get_parameter("save_nav_log", m_save_nav_log);
 	RCLCPP_INFO(
 		this->get_logger(), "save_nav_log: %s", m_save_nav_log ? "yes" : "no");
 
-	this->declare_parameter<std::string>("ptg_plugin_files", "");
-	this->get_parameter("ptg_plugin_files", m_plugin_file);
+	declare_parameter<std::string>("ptg_plugin_files", "");
+	get_parameter("ptg_plugin_files", m_plugin_file);
 	RCLCPP_INFO(
 		this->get_logger(), "ptg_plugin_files: %s", m_plugin_file.c_str());