Added services to start and stop processing of sensor data.

CMakeLists.txt

@@ -55,6 +55,8 @@ add_message_files(
 add_service_files(
     FILES
         ReloadLocalizationConfiguration.srv
+        StartProcessingSensorData.srv
+        StopProcessingSensorData.srv
 )
 
 generate_messages(

include/dynamic_robot_localization/localization/impl/localization.hpp

@@ -18,6 +18,7 @@ namespace dynamic_robot_localization {
 // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <constructors-destructor>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
 template<typename PointT>
 Localization<PointT>::Localization() :
+	ambient_pointcloud_topic_disabled_on_startup_(false),
 	ambient_pointcloud_integration_filters_preprocessed_pointcloud_save_original_pointcloud_(true),
 	filtered_pointcloud_save_frame_id_with_cloud_time_(false),
 	stop_processing_after_saving_filtered_pointcloud_(true),
@@ -165,6 +166,29 @@ bool Localization<PointT>::reloadConfigurationFromParameterServerServiceCallback
 }
 
 
+template<typename PointT>
+bool Localization<PointT>::startProcessingSensorDataServiceCallback(dynamic_robot_localization::StartProcessingSensorData::Request& request, dynamic_robot_localization::StartProcessingSensorData::Response& response) {
+	bool status = false;
+	if (request.set_initial_pose) {
+		status = setInitialPose(request.initial_pose.pose, request.initial_pose.header.frame_id, request.initial_pose.header.stamp);
+	} else {
+		last_accepted_pose_time_ = ros::Time::now();
+		status = true;
+	}
+	restartProcessingSensorData();
+	response.status = status;
+	return true;
+}
+
+
+template<typename PointT>
+bool Localization<PointT>::stopProcessingSensorDataServiceCallback(dynamic_robot_localization::StopProcessingSensorData::Request& request, dynamic_robot_localization::StopProcessingSensorData::Response& response) {
+	stopProcessingSensorData();
+	response.status = true;
+	return true;
+}
+
+
 template<typename PointT>
 bool Localization<PointT>::reloadConfigurationFromParameterServer(const dynamic_robot_localization::LocalizationConfiguration& localization_configuration) {
 	std::string parsed_string;
@@ -285,6 +309,7 @@ void Localization<PointT>::setupSubscribeTopicNamesFromParameterServer(const std
 	private_node_handle_->param(configuration_namespace + "subscribe_topic_names/pose_stamped_topic", pose_stamped_topic_, std::string("initial_pose_stamped"));
 	private_node_handle_->param(configuration_namespace + "subscribe_topic_names/pose_with_covariance_stamped_topic", pose_with_covariance_stamped_topic_, std::string("/initialpose"));
 	private_node_handle_->param(configuration_namespace + "subscribe_topic_names/ambient_pointcloud_topic", ambient_pointcloud_topics_, std::string("ambient_pointcloud"));
+	private_node_handle_->param(configuration_namespace + "subscribe_topic_names/ambient_pointcloud_topic_disabled_on_startup", ambient_pointcloud_topic_disabled_on_startup_, false);
 	private_node_handle_->param(configuration_namespace + "subscribe_topic_names/reference_costmap_topic", reference_costmap_topic_, std::string("/map"));
 	private_node_handle_->param(configuration_namespace + "subscribe_topic_names/reference_pointcloud_topic", reference_pointcloud_topic_, std::string(""));
 }
@@ -293,6 +318,8 @@ template<typename PointT>
 void Localization<PointT>::setupServiceServersNamesFromParameterServer(const std::string &configuration_namespace) {
 	ROS_DEBUG_STREAM("Loading [service_servers_names] configurations from parameter server namespace [" << (configuration_namespace.empty() ? "~" : configuration_namespace) << "]");
 	private_node_handle_->param(configuration_namespace + "service_servers_names/reload_localization_configuration_service_server_name", reload_localization_configuration_service_server_name_, std::string("reload_localization_configuration"));
+	private_node_handle_->param(configuration_namespace + "service_servers_names/start_processing_sensor_data_service_server_name", start_processing_sensor_data_service_server_name_, std::string("start_processing_sensor_data"));	
+	private_node_handle_->param(configuration_namespace + "service_servers_names/stop_processing_sensor_data_service_server_name", stop_processing_sensor_data_service_server_name_, std::string("stop_processing_sensor_data"));
 }
 
 template<typename PointT>
@@ -1363,7 +1390,7 @@ void Localization<PointT>::updateMatchersReferenceCloud() {
 
 
 template<typename PointT>
-void Localization<PointT>::setInitialPose(const geometry_msgs::Pose& pose, const std::string& frame_id, const ros::Time& pose_time) {
+bool Localization<PointT>::setInitialPose(const geometry_msgs::Pose& pose, const std::string& frame_id, const ros::Time& pose_time) {
 	ros::Time pose_time_updated = pose_time;
 	if ((pose_time.sec == 0 && pose_time.nsec == 0) || pose_time.toSec() < 3.0) {
 		pose_time_updated = ros::Time::now();
@@ -1377,7 +1404,7 @@ void Localization<PointT>::setInitialPose(const geometry_msgs::Pose& pose, const
 
 		if (!math_utils::isTransformValid(transform_base_link_to_map)) {
 			ROS_WARN("Discarded initial pose with NaN values!");
-			return;
+			return false;
 		}
 
 		if (invert_initial_poses_from_msgs_)
@@ -1398,7 +1425,7 @@ void Localization<PointT>::setInitialPose(const geometry_msgs::Pose& pose, const
 			tf2::Transform last_accepted_pose_odom_to_map = transform_base_link_to_map * transform_odom_to_base_link;
 			if (!math_utils::isTransformValid(last_accepted_pose_odom_to_map)) {
 				ROS_WARN("Discarded initial pose because the multiplication of [transform_base_link_to_map * transform_odom_to_base_link] resulted in a transform with NaN values!");
-				return;
+				return false;
 			}
 
 			last_accepted_pose_base_link_to_map_ = transform_base_link_to_map;
@@ -1410,12 +1437,16 @@ void Localization<PointT>::setInitialPose(const geometry_msgs::Pose& pose, const
 			ROS_INFO_STREAM("Received initial pose at time [" << pose_time_updated << "]: " \
 					<< "\n\tTranslation -> [ x: " << transform_base_link_to_map.getOrigin().getX() << " | y: " << transform_base_link_to_map.getOrigin().getY() << " | z: " << transform_base_link_to_map.getOrigin().getZ() << " ]" \
 					<< "\n\tRotation -> [ qx: " << transform_base_link_to_map.getRotation().getX() << " | qy: " << transform_base_link_to_map.getRotation().getY() << " | qz: " << transform_base_link_to_map.getRotation().getZ() << " | qw: " << transform_base_link_to_map.getRotation().getW() << " ]");
+
+			return true;
 		} else {
 			ROS_WARN_STREAM("Discarded initial pose because there is no TF from frame [" << base_link_frame_id_ << "] to frame [" << odom_frame_id_ << "]");
 		}
 	} else {
 		ROS_WARN_STREAM("Discarded initial pose because it should be in [" << map_frame_id_ << "] frame instead of [" << frame_id << "] frame");
 	}
+
+	return false;
 }
 
 
@@ -1565,12 +1596,15 @@ void Localization<PointT>::startSubscribers() {
 		std::stringstream ss(ambient_pointcloud_topics_);
 		std::string topic_name;
 
+		ambient_pointcloud_topic_names_.clear();
 		while (ss >> topic_name && !topic_name.empty()) {
-			ambient_pointcloud_subscribers_.push_back(node_handle_->subscribe(topic_name, 1, &dynamic_robot_localization::Localization<PointT>::processAmbientPointCloud, this));
+			ambient_pointcloud_topic_names_.push_back(topic_name);
 			ROS_INFO_STREAM("Adding " << topic_name << " to the list of sensor_msgs::PointCloud2 topics to use in localization");
 		}
 
-		ambient_pointcloud_subscribers_active_ = true;
+		if (!ambient_pointcloud_topic_disabled_on_startup_) {
+			restartProcessingSensorData();
+		}
 	}
 }
 
@@ -1582,6 +1616,18 @@ void Localization<PointT>::startServiceServers() {
 	} else {
 		reload_localization_configuration_service_server_.shutdown();
 	}
+
+	if (!start_processing_sensor_data_service_server_name_.empty()) {
+		start_processing_sensor_data_service_server_ = node_handle_->advertiseService(start_processing_sensor_data_service_server_name_, &dynamic_robot_localization::Localization<PointT>::startProcessingSensorDataServiceCallback, this);
+	} else {
+		start_processing_sensor_data_service_server_.shutdown();
+	}
+
+	if (!stop_processing_sensor_data_service_server_name_.empty()) {
+		stop_processing_sensor_data_service_server_ = node_handle_->advertiseService(stop_processing_sensor_data_service_server_name_, &dynamic_robot_localization::Localization<PointT>::stopProcessingSensorDataServiceCallback, this);
+	} else {
+		stop_processing_sensor_data_service_server_.shutdown();
+	}
 }
 
 
@@ -1628,25 +1674,19 @@ void Localization<PointT>::stopProcessingSensorData() {
 	for (size_t i = 0; i < ambient_pointcloud_subscribers_.size(); ++i) {
 		ambient_pointcloud_subscribers_[i].shutdown();
 	}
+	ambient_pointcloud_subscribers_.clear();
 }
 
 
 template<typename PointT>
 void Localization<PointT>::restartProcessingSensorData() {
+	stopProcessingSensorData();
 	resetNumberOfProcessedPointclouds();
 	ambient_pointcloud_subscribers_active_ = true;
 	sensor_data_processing_status_ = WaitingForSensorData;
-	std::vector<std::string> topic_names;
-
-	for (size_t i = 0; i < ambient_pointcloud_subscribers_.size(); ++i) {
-		topic_names.push_back(ambient_pointcloud_subscribers_[i].getTopic());
-		ambient_pointcloud_subscribers_[i].shutdown();
-	}
-
-	ambient_pointcloud_subscribers_.clear();
 
-	for (size_t i = 0; i < topic_names.size(); ++i) {
-		ambient_pointcloud_subscribers_.push_back(node_handle_->subscribe(topic_names[i], 1, &dynamic_robot_localization::Localization<PointT>::processAmbientPointCloud, this));
+	for (size_t i = 0; i < ambient_pointcloud_topic_names_.size(); ++i) {
+		ambient_pointcloud_subscribers_.push_back(node_handle_->subscribe(ambient_pointcloud_topic_names_[i], 1, &dynamic_robot_localization::Localization<PointT>::processAmbientPointCloud, this));
 	}
 }
 

include/dynamic_robot_localization/localization/localization.h

@@ -127,6 +127,8 @@
 #include <dynamic_robot_localization/LocalizationTimes.h>
 #include <dynamic_robot_localization/LocalizationConfiguration.h>
 #include <dynamic_robot_localization/ReloadLocalizationConfiguration.h>
+#include <dynamic_robot_localization/StartProcessingSensorData.h>
+#include <dynamic_robot_localization/StopProcessingSensorData.h>
 // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   </includes>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
 
 namespace dynamic_robot_localization {
@@ -212,6 +214,8 @@ class Localization : public ConfigurableObject {
 		// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>   <Localization-functions>   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
 		virtual void setupConfigurationFromParameterServer(ros::NodeHandlePtr& node_handle, ros::NodeHandlePtr& private_node_handle, const std::string& configuration_namespace);
 		virtual bool reloadConfigurationFromParameterServerServiceCallback(dynamic_robot_localization::ReloadLocalizationConfiguration::Request& request, dynamic_robot_localization::ReloadLocalizationConfiguration::Response& response);
+		virtual bool startProcessingSensorDataServiceCallback(dynamic_robot_localization::StartProcessingSensorData::Request& request, dynamic_robot_localization::StartProcessingSensorData::Response& response);
+		virtual bool stopProcessingSensorDataServiceCallback(dynamic_robot_localization::StopProcessingSensorData::Request& request, dynamic_robot_localization::StopProcessingSensorData::Response& response);
 		virtual bool reloadConfigurationFromParameterServer(const dynamic_robot_localization::LocalizationConfiguration& localization_configuration);
 		virtual void setupGeneralConfigurationsFromParameterServer(const std::string& configuration_namespace);
 		virtual void setupSubscribeTopicNamesFromParameterServer(const std::string &configuration_namespace);
@@ -288,7 +292,7 @@ class Localization : public ConfigurableObject {
 		virtual bool updateLocalizationPipelineWithNewReferenceCloud(const ros::Time& time_stamp);
 		virtual void updateMatchersReferenceCloud();
 
-		virtual void setInitialPose(const geometry_msgs::Pose& pose, const std::string& frame_id, const ros::Time& pose_time);
+		virtual bool setInitialPose(const geometry_msgs::Pose& pose, const std::string& frame_id, const ros::Time& pose_time);
 		virtual void setInitialPoseFromPose(const geometry_msgs::PoseConstPtr& pose);
 		virtual void setInitialPoseFromPoseStamped(const geometry_msgs::PoseStampedConstPtr& pose);
 		virtual void setInitialPoseFromPoseWithCovarianceStamped(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose);
@@ -411,11 +415,15 @@ class Localization : public ConfigurableObject {
 		std::string pose_stamped_topic_;
 		std::string pose_with_covariance_stamped_topic_;
 		std::string ambient_pointcloud_topics_;
+		std::vector<std::string> ambient_pointcloud_topic_names_;
+		bool ambient_pointcloud_topic_disabled_on_startup_;
 		std::string reference_pointcloud_topic_;
 		std::string reference_costmap_topic_;
 
 		// service servers
 		std::string reload_localization_configuration_service_server_name_;
+		std::string start_processing_sensor_data_service_server_name_;
+		std::string stop_processing_sensor_data_service_server_name_;
 
 		// publish topic names
 		std::string reference_pointcloud_publish_topic_;
@@ -541,6 +549,8 @@ class Localization : public ConfigurableObject {
 		ros::Subscriber pose_stamped_subscriber_;
 		ros::Subscriber pose_with_covariance_stamped_subscriber_;
 		ros::ServiceServer reload_localization_configuration_service_server_;
+		ros::ServiceServer start_processing_sensor_data_service_server_;
+		ros::ServiceServer stop_processing_sensor_data_service_server_;
 		std::vector< ros::Subscriber > ambient_pointcloud_subscribers_;
 		bool ambient_pointcloud_subscribers_active_;
 		int limit_of_pointclouds_to_process_;

launch/dynamic_robot_localization_system.launch

@@ -51,6 +51,7 @@
 
 	<!-- sensor data from point cloud topic -->
 	<arg name="ambient_pointcloud_topic" default="ambient_pointcloud" />
+	<arg name="ambient_pointcloud_topic_disabled_on_startup" default="false" />
 	<arg name="imu_topic" default="imu" />
 
 	<!-- initial pose setup -->
@@ -73,7 +74,7 @@
 	<arg name="laser_scan_topics" default="/tilt_scan+/base_scan" />
 	<arg name="min_range_cutoff_percentage_offset" default="2.00" />
 	<arg name="max_range_cutoff_percentage_offset" default="0.95" />
-	<arg name="laser_asssembler_remove_invalid_measurements" default="true" />
+	<arg name="laser_assembler_remove_invalid_measurements" default="true" />
 	<arg name="number_of_tf_queries_for_spherical_interpolation" default="4" />
 	<arg name="number_of_scans_to_assemble_per_cloud" default="2" />
 	<arg name="timeout_for_cloud_assembly" default="0.5" />
@@ -92,7 +93,7 @@
 	<arg name="publish_initial_pose" default="true" />
 	<arg name="publish_pose_tf_rate" default="-10.0" />
 	<arg name="tf_lookup_timeout" default="0.1" />
-	<arg name="publish_last_pose_tf_timeout_seconds" default="3.0" />
+	<arg name="publish_last_pose_tf_timeout_seconds" default="3.0" /> <!-- set to negative value for publishing a tf only when drl estimates a new pose -->
 	<arg name="invert_tf_transform" default="false" />
 	<arg name="invert_tf_hierarchy" default="false" />
 	<arg name="transform_pose_to_map_frame_id" default="true" />
@@ -135,6 +136,7 @@
 		<param name="subscribe_topic_names/pose_stamped_topic" type="str" value="$(arg pose_stamped_topic)" />
 		<param name="subscribe_topic_names/pose_with_covariance_stamped_topic" type="str" value="$(arg pose_with_covariance_stamped_topic)" />
 		<param name="subscribe_topic_names/ambient_pointcloud_topic" type="str" value="$(arg ambient_pointcloud_topic)" />
+		<param name="subscribe_topic_names/ambient_pointcloud_topic_disabled_on_startup" type="bool" value="$(arg ambient_pointcloud_topic_disabled_on_startup)" />
 		<param name="subscribe_topic_names/reference_costmap_topic" type="str" value="$(arg reference_costmap_topic)" />
 		<param name="subscribe_topic_names/reference_pointcloud_topic" type="str" value="$(arg reference_pointcloud_topic)" />
 		<param name="publish_topic_names/pose_with_covariance_stamped_publish_topic" type="str" value="$(arg pose_with_covariance_stamped_publish_topic)" />
@@ -207,7 +209,7 @@
 		<arg name="number_of_tf_queries_for_spherical_interpolation" default="$(arg number_of_tf_queries_for_spherical_interpolation)" />
 		<arg name="number_of_scans_to_assemble_per_cloud" default="$(arg number_of_scans_to_assemble_per_cloud)" />
 		<arg name="timeout_for_cloud_assembly" default="$(arg timeout_for_cloud_assembly)" />
-		<arg name="remove_invalid_measurements" default="$(arg laser_asssembler_remove_invalid_measurements)" />
+		<arg name="remove_invalid_measurements" default="$(arg laser_assembler_remove_invalid_measurements)" />
 		<arg name="dynamic_update_of_assembly_configuration_from_twist_topic" default="$(arg dynamic_update_of_assembly_configuration_from_twist_topic)" />
 		<arg name="dynamic_update_of_assembly_configuration_from_odometry_topic" default="$(arg dynamic_update_of_assembly_configuration_from_odometry_topic)" />
 		<arg name="dynamic_update_of_assembly_configuration_from_imu_topic" default="$(arg dynamic_update_of_assembly_configuration_from_imu_topic)" />

srv/StartProcessingSensorData.srv

@@ -0,0 +1,4 @@
+geometry_msgs/PoseStamped initial_pose
+bool set_initial_pose
+---
+bool status

srv/StopProcessingSensorData.srv

@@ -0,0 +1,2 @@
+---
+bool status

yaml/schema/drl_configs.yaml

@@ -69,11 +69,14 @@ subscribe_topic_names:
     pose_stamped_topic: 'initial_pose_stamped'                                  # geometry_msgs::PoseStamped | Topic to reset localization pose
     pose_with_covariance_stamped_topic: '/initialpose'                          # geometry_msgs::PoseWithCovarianceStamped | Topic to reset localization pose (rviz topic name)
     ambient_pointcloud_topic: 'ambient_pointcloud'                              # sensor_msgs::PointCloud2 | Cloud with points coming from sensing devices that will be used to update the localization pose (multiple topics are supported, by merging their names with + -> ex: cloud1+cloud2+cloud3)
+    ambient_pointcloud_topic_disabled_on_startup: false                         # if true, drl will not start the sensor data subscribers on startup
     reference_costmap_topic: '/map'                                             # nav_msgs::OccupancyGrid  | Topic providing a 2D reference map
     reference_pointcloud_topic: ''                                              # sensor_msgs::PointCloud2 | Topic providing a 3D reference map -> OctoMap is configured to use topic 'reference_pointcloud_update'
 
 service_servers_names:
     reload_localization_configuration_service_server_name: "reload_localization_configuration"
+    start_processing_sensor_data_service_server_name: "start_processing_sensor_data"
+    stop_processing_sensor_data_service_server_name: "stop_processing_sensor_data"
 
 
 # ===================================================================================================================================================