Ask Your Question
1

Change simulator stage model configurations in runtime

asked 2011-12-17 02:34:06 -0600

updated 2011-12-17 09:20:07 -0600

Can I change the stage/stageros robot models properties in runtime wihtout restarting the stage simulator?

edit retag flag offensive close merge delete

Comments

Is this a question?
joq gravatar imagejoq ( 2011-12-17 07:15:05 -0600 )edit
Currently there is no support for this feature. Maybe somebdoy need it. Here you also can find the answer about how to solve the problem.
Pablo Iñigo Blasco gravatar imagePablo Iñigo Blasco ( 2011-12-17 09:11:28 -0600 )edit
I've refactored the entry to clarify what is the question and what is the answer.
Pablo Iñigo Blasco gravatar imagePablo Iñigo Blasco ( 2011-12-17 09:21:43 -0600 )edit
@Pablo Iñigo Blasco : I am using Stage (as Player/Stage and also as ROS/stageros) for about 2 years now, I am very apprehensive of any solution to your problem. I follow your question most keenly.
Arkapravo gravatar imageArkapravo ( 2011-12-19 23:20:12 -0600 )edit
The attatched patch in my own answer is a solution for the problem itsef. As Perco suggested I requested this enhacement. I only focused on laser config so this patch can be improved. Almost any aspect of Stage can be reconfigured in runtime using this template - Bridge ros-service C++Stage API
Pablo Iñigo Blasco gravatar imagePablo Iñigo Blasco ( 2011-12-20 01:59:31 -0600 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2011-12-17 09:21:05 -0600

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 ...
(more)
edit flag offensive delete link more

Comments

I suggest you open an enhancement ticket against the stage package and attach the patch to that. That way, this patch won't get lost and may make it into a future release of the stage package. Please update your answer to include that trac link once you create the ticket.
Eric Perko gravatar imageEric Perko ( 2011-12-17 15:34:44 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2011-12-17 02:34:06 -0600

Seen: 669 times

Last updated: Dec 17 '11