Change simulator stage model configurations in runtime

edit

This is not currently supported.

However here you can find a partial solution.

Here I attached a patch over the stage package which provides a service to change elemental attributes of lasers beams of the robot models. This service can be also a template for further runtime configuration changes.

Example:

import roslib
roslib.load_manifest('stage')
import rospy
import stage.msg
import stage.srv
stage_model_config_srv=rospy.ServiceProxy('/stageros/set_models_configurations',stage.srv.SetModelConfig)

model_config=stage.msg.ModelConfig()
model_config.laser_fov=-1.0
model_config.laser_range_max=-1
model_config.laser_range_min=-1
model_config.laser_resolution=-1
model_config.laser_samples=8
stage_model_config_srv(models_configurations=[model_config])

It was useful for some test bencharks in my research and maybe will be useful for someone else.

But the below diff text is a good resume of the changes made. However the attached patch (rename from it to .diff) C:\fakepath\stage_configuration_service.diff.jpg should be used. It is a bit more verbose (a lot of indentation eclipse-ros format changes)

This patch have been applied over:

URL: https://code.ros.org/svn/ros-pkg/stacks/stage/trunk
Repository Root: https://code.ros.org/svn/ros-pkg
Repository UUID: eb33c2ac-9c88-4c90-87e0-44a10359b0c3
Revision: 38364

Diff text:

Index: msg/ModelConfig.msg
===================================================================
--- msg/ModelConfig.msg (revision 0)
+++ msg/ModelConfig.msg (revision 0)
@@ -0,0 +1,8 @@
+#geometry_msgs/Pose2D laser_pose
+#to maintiain its current value set each config property to -1 
+
+int32 laser_samples
+int32 laser_resolution
+float64 laser_fov
+int32 laser_range_max
+int32 laser_range_min
Index: src/stageros.cpp
===================================================================
--- src/stageros.cpp    (revision 38364)
+++ src/stageros.cpp    (working copy)
@@ -47,6 +47,7 @@
 #include <rosgraph_msgs clock.h="">

 #include "tf/transform_broadcaster.h"
+#include "stage/SetModelConfig.h"

 #define USAGE "stageros <worldfile>"
 #define ODOM "odom"
@@ -79,6 +80,9 @@
     std::vector<ros::subscriber> cmdvel_subs_;
     ros::Publisher clock_pub_;

+    ros::ServiceServer models_configurations_service_;
+    bool model_config_callback(stage::SetModelConfig::Request& request, stage::SetModelConfig::Response& response);
+
     // A helper function that is executed for each stage model.  We use it
     // to search for models of interest.
     static void ghfunc(Stg::Model* mod, StageNode* node);
@@ -213,9 +217,47 @@
   this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
   this->odomMsgs = new nav_msgs::Odometry[numRobots];
   this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
+
+  this->models_configurations_service_ = localn.advertiseService("set_models_configurations", &StageNode::model_config_callback, this);
 }

+bool StageNode::model_config_callback(stage::SetModelConfig::Request& request, stage::SetModelConfig::Response& response)
+{
+  boost::mutex::scoped_lock lock(msg_lock);

+  if(request.models_configurations.size()>lasermodels.size())
+  {
+    ROS_ERROR("Service stage model config: configurations.count > simulator_models.count");
+    return false;
+  }
+
+  for(unsigned int i=0;i<request.models_configurations.size();i++) +="" {="" +="" stage::modelconfig&="" current_msg_config="request.models_configurations[i];" +="" stg::modellaser::config="" laser_config="this-">lasermodels[i]->GetConfig();
+    if(current_msg_config.laser_samples!=-1)
+      laser_config.sample_count=current_msg_config.laser_samples; ///< Number of range samples
+
+    if(current_msg_config.laser_resolution!=-1)
+      laser_config.resolution=current_msg_config.laser_resolution; ///< interpolation
+
+    if(current_msg_config.laser_range_min!=-1)
+      laser_config.range_bounds.min=current_msg_config.laser_range_min; ///< min and max ranges
+
+    if(current_msg_config.laser_range_max!=-1)
+      laser_config.range_bounds.max=current_msg_config.laser_range_max;
+
+    if(current_msg_config.laser_fov!=-1.0)
+      laser_config.fov=current_msg_config.laser_fov; ///< Field of view, centered about the pose angle
+
+    //stg_usec_t interval;
+
+    lasermodels[i]->SetConfig(laser_config);
+  }
+
+  return true;
+}
+
 // Subscribe to models of interest.  Currently, we find and subscribe
 // to the first 'laser' model and the first 'position' model.  Returns
 // 0 on success (both models subscribed), -1 ...