added ROS parameter continuous sensor pose update

mrpt-ros-pkg · Mar 3, 2020 · a6cd8fa · a6cd8fa
1 parent e9145c8
commit a6cd8fa
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 1 deletion.
diff --git a/mrpt_localization/cfg/Motion.cfg b/mrpt_localization/cfg/Motion.cfg
@@ -20,5 +20,6 @@ gen.add("default_noise_xy", double_t, 0, "default_noise_xy", 0.1, 0, 1)
 gen.add("default_noise_phi", double_t, 0, "default_noise_phi", 2.0, 0, 3.14)
 
 gen.add("update_while_stopped", bool_t, 0, "keep updating filter while the robot is stopped", True)
+gen.add("update_sensor_pose", bool_t, 0, "keep updating the sensor pose using the tf tree", False)
 
 exit(gen.generate(PACKAGE, "mrpt_localization", "Motion"))
diff --git a/mrpt_localization/include/mrpt_localization_node.h b/mrpt_localization/include/mrpt_localization_node.h
@@ -90,6 +90,7 @@ class PFLocalizationNode : public PFLocalization
 		std::string odom_frame_id;
 		std::string global_frame_id;
 		bool update_while_stopped;
+		bool update_sensor_pose;
 		bool pose_broadcast;
 		bool tf_broadcast;
 		bool use_map_topic;

diff --git a/mrpt_localization/src/mrpt_localization_node.cpp b/mrpt_localization/src/mrpt_localization_node.cpp
@@ -211,7 +211,7 @@ void PFLocalizationNode::callbackLaser(const sensor_msgs::LaserScan& _msg)
 
 	// ROS_INFO("callbackLaser");
 	auto laser = CObservation2DRangeScan::Create();
-
+    
 	// printf("callbackLaser %s\n", _msg.header.frame_id.c_str());
 	if (laser_poses_.find(_msg.header.frame_id) == laser_poses_.end())
 	{
@@ -220,6 +220,9 @@ void PFLocalizationNode::callbackLaser(const sensor_msgs::LaserScan& _msg)
 	else if (state_ != IDLE)  // updating filter; we must be moving or
 	// update_while_stopped set to true
 	{
+        if(param()->update_sensor_pose) {
+            updateSensorPose(_msg.header.frame_id);
+        }
 		// mrpt::poses::CPose3D pose = laser_poses_[_msg.header.frame_id];
 		// ROS_INFO("LASER POSE %4.3f, %4.3f, %4.3f, %4.3f, %4.3f, %4.3f",
 		// pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
@@ -254,6 +257,9 @@ void PFLocalizationNode::callbackBeacon(
 	else if (state_ != IDLE)  // updating filter; we must be moving or
 	// update_while_stopped set to true
 	{
+        if(param()->update_sensor_pose) {
+            updateSensorPose(_msg.header.frame_id);
+        }
 		// mrpt::poses::CPose3D pose = beacon_poses_[_msg.header.frame_id];
 		// ROS_INFO("BEACON POSE %4.3f, %4.3f, %4.3f, %4.3f, %4.3f, %4.3f",
 		// pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());

diff --git a/mrpt_localization/src/mrpt_localization_node_parameters.cpp b/mrpt_localization/src/mrpt_localization_node_parameters.cpp
@@ -160,4 +160,7 @@ void PFLocalizationNode::Parameters::callbackParameters(
 	update_while_stopped = config.update_while_stopped;
 	ROS_INFO(
 		"update_while_stopped: %s", update_while_stopped ? "true" : "false");
+	update_sensor_pose = config.update_sensor_pose;
+	ROS_INFO(
+		"update_sensor_pose: %s", update_sensor_pose ? "true" : "false");
 }