Stage
Stage copied to clipboard
rtv
Adding cameras to stageros
Willow Garage is migrating code repository & issue tracker to github and I haven't found any other location for Stage than here. If it's not a right space for ROS binding please say so. Thank you.
Migrated from https://code.ros.org/trac/ros/ticket/4058 Reported by: trinighost Owned by: gerkey Priority: major Last modified: Nov 2012
Description There are a couple of things that I wish I had more time to address, these include using the image_transport, and addressing the matter of whether a camera can exist with out a host model, but here's a patch that provides the missing functionality.
Content of camera_rev.patch
TabularUnified src/stageros.cpp
42 42 #include <boost/thread/mutex.hpp>
43 43 #include <boost/thread/thread.hpp>
44 44 #include <sensor_msgs/LaserScan.h>
45 #include <sensor_msgs/Image.h>
45 46 #include <nav_msgs/Odometry.h>
46 47 #include <geometry_msgs/Twist.h>
47 48 #include <rosgraph_msgs/Clock.h>
… …
49 50 #include "tf/transform_broadcaster.h"
50 51
51 52 #define USAGE "stageros <worldfile>"
53 #define IMAGE "image"
52 54 #define ODOM "odom"
53 55 #define BASE_SCAN "base_scan"
54 56 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
… …
59 61 {
60 62 private:
61 63 // Messages that we'll send or receive
64 sensor_msgs::Image *imageMsgs;
62 65 sensor_msgs::LaserScan *laserMsgs;
63 66 nav_msgs::Odometry *odomMsgs;
64 67 nav_msgs::Odometry *groundTruthMsgs;
… …
71 74 boost::mutex msg_lock;
72 75
73 76 // The models that we're interested in
77 std::vector<Stg::ModelCamera *> cameramodels;
74 78 std::vector<Stg::ModelRanger *> lasermodels;
75 79 std::vector<Stg::ModelPosition *> positionmodels;
80 std::vector<ros::Publisher> image_pubs_;
76 81 std::vector<ros::Publisher> laser_pubs_;
77 82 std::vector<ros::Publisher> odom_pubs_;
78 83 std::vector<ros::Publisher> ground_truth_pubs_;
… …
150 155 node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
151 156 if (dynamic_cast<Stg::ModelPosition *>(mod))
152 157 node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod));
158 if (dynamic_cast<Stg::ModelCamera *>(mod))
159 node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
153 160 }
154 161
155 162 void
… …
186 193 Stg::Init( &argc, &argv );
187 194
188 195 if(gui)
189 this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
196 this->world = new Stg::WorldGui(400, 300, "Stage (ROS)");
190 197 else
191 198 this->world = new Stg::World();
192 199
… …
207 214 "the world file.");
208 215 ROS_BREAK();
209 216 }
217 else if (cameramodels.size() > 0 && cameramodels.size() != positionmodels.size())
218 {
219 ROS_FATAL("number of position models and camera models must be equal in "
220 "the world file.");
221 ROS_BREAK();
222 }
223
210 224 size_t numRobots = positionmodels.size();
211 ROS_INFO("found %u position/laser pair%s in the file",
225 ROS_INFO("found %u set%s of position/laser/camera in the file",
212 226 (unsigned int)numRobots, (numRobots==1) ? "" : "s");
213 227
214 228 this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
215 229 this->odomMsgs = new nav_msgs::Odometry[numRobots];
216 230 this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
231 this->imageMsgs = new sensor_msgs::Image[numRobots];
217 232 }
218 233
219 234
… …
248 263 ROS_ERROR("no position");
249 264 return(-1);
250 265 }
266 if(this->cameramodels[r])
267 {
268 this->cameramodels[r]->Subscribe();
269 }
270 else
271 {
272 ROS_ERROR("no camera");
273 return(-1);
274 }
251 275 laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
252 276 odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
253 277 ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
278 image_pubs_.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE,r), 10));
254 279 cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
255 280 }
256 281 clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
… …
262 287 delete[] laserMsgs;
263 288 delete[] odomMsgs;
264 289 delete[] groundTruthMsgs;
290 delete[] imageMsgs;
265 291 }
266 292
267 293 bool
… …
345 371 mapName("base_footprint", r),
346 372 mapName("base_link", r)));
347 373
374 // Get latest image data
375 // Translate into ROS message format and publish
376 if (this->cameramodels[r]->FrameColor()) {
377 this->imageMsgs[r].height=this->cameramodels[r]->getHeight();
378 this->imageMsgs[r].width=this->cameramodels[r]->getWidth();
379 this->imageMsgs[r].encoding="rgba8";
380 //this->imageMsgs[r].is_bigendian="";
381 this->imageMsgs[r].step=this->imageMsgs[r].width*4;
382 this->imageMsgs[r].data.resize(this->imageMsgs[r].width*this->imageMsgs[r].height*4);
383
384 memcpy(&(this->imageMsgs[r].data[0]),this->cameramodels[r]->FrameColor(),this->imageMsgs[r].width*this->imageMsgs[r].height*4);
385
386 //invert the opengl weirdness
387 int height = this->imageMsgs[r].height - 1;
388 int linewidth = this->imageMsgs[r].width*4;
389
390 char* temp = new char[linewidth];
391 for (int y = 0; y < (height+1)/2; y++)
392 {
393 memcpy(temp,&this->imageMsgs[r].data[y*linewidth],linewidth);
394 memcpy(&(this->imageMsgs[r].data[y*linewidth]),&(this->imageMsgs[r].data[(height-y)*linewidth]),linewidth);
395 memcpy(&(this->imageMsgs[r].data[(height-y)*linewidth]),temp,linewidth);
396 }
397
398 this->imageMsgs[r].header.frame_id = mapName("image", r);
399 this->imageMsgs[r].header.stamp = sim_time;
400
401 this->image_pubs_[r].publish(this->imageMsgs[r]);
402 }
403
348 404 // Get latest odometry data
349 405 // Translate into ROS message format and publish
350 406 this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
TabularUnified world/willow-four-erratics.world
4 4 gui_nose 0
5 5 )
6 6
7 define topurg laser
7 define topurg ranger
8 8 (
9 sensor(
10 range [ 0.0 30.0 ]
11 fov 270.25
12 samples 1081
13 )
9 14
10 range_max 30.0
11 fov 270.25
12 samples 1081
13 15 # generic model properties
14 16 color "black"
15 17 size [ 0.05 0.05 0.1 ]
… …
23 25 gui_nose 1
24 26 drive "diff"
25 27 topurg(pose [ 0.050 0.000 0 0.000 ])
28
29 camera
30 (
31 # laser properties
32 resolution [ 300 400 ]
33 range [ 0.2 8.0 ]
34 fov [ 70.0 40.0 ]
35 pantilt [ 0.0 0.0 ]
36
37 # model properties
38 size [ 0.1 0.07 0.05 ]
39 color "black"
40 watts 100.0 # TODO find watts for sony pan-tilt camera
41 )
26 42 )
27 43
28 44 define floorplan model
TabularUnified world/willow-erratic.world
1
1 2 define block model
2 3 (
3 4 size [0.5 0.5 0.5]
… …
25 26 gui_nose 1
26 27 drive "diff"
27 28 topurg(pose [ 0.050 0.000 0 0.000 ])
29
30 camera
31 (
32 # laser properties
33 resolution [ 32 32 ]
34 range [ 0.2 8.0 ]
35 fov [ 70.0 40.0 ]
36 pantilt [ 0.0 0.0 ]
37
38 # model properties
39 size [ 0.1 0.07 0.05 ]
40 color "black"
41 watts 100.0 # TODO find watts for sony pan-tilt camera
42 )
43
28 44 )
29 45
30 46 define floorplan model
After the camera_rev patch camera module is a required for each simulated robot. But in the patched file stageros.cc Line217, has little bug .Make stageros fail to detect the when the stage robot has no camera with it. And the result is segment fault after running stageros. Here is the fix origional patch 217 else if (cameramodels.size() > 0 && cameramodels.size() != positionmodels.size()) change to 217 else if ( cameramodels.size() != positionmodels.size())
rtv/Stage is the canonical repo for Stage.
Thanks for the contributions! I plan to work on the ROS interface quite a bit in the next few months.
Richard/
On Thu, Jan 24, 2013 at 6:13 AM, bombilee [email protected] wrote:
After the camera patch the camera module is a required for each simulated robot. But in the patched file stageros.cc Line217, has little bug .Make stageros fail to detect the when the stage robot has no camera with it. And the result is segment fault after running stageros. Here is the fix origional patched
217 else if (cameramodels.size() > 0 && cameramodels.size() != positionmodels.size()) change to 217 else if ( cameramodels.size() != positionmodels.size())
— Reply to this email directly or view it on GitHubhttps://github.com/rtv/Stage/issues/34#issuecomment-12652624.
