Adding cameras to stageros

[130s]

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 

[bombilee]

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]

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//issues/34#issuecomment-12652624.