ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

code.ros is having certificate issues, so until that gets resolved I'm sharing this here. There are a couple of things that I wish I had more time to address, these include using the image_transport, inverting the image, addressing the matter of whether a camera can exist with out a host model.

As always:

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

The proposed changes are as follows:

Index: src/stageros.cpp
===================================================================
--- src/stageros.cpp    (revision 40069)
+++ src/stageros.cpp    (working copy)
@@ -42,6 +42,7 @@
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/thread.hpp>
 #include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/Image.h>
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/Twist.h>
 #include <rosgraph_msgs/Clock.h>
@@ -49,6 +50,7 @@
 #include "tf/transform_broadcaster.h"

 #define USAGE "stageros <worldfile>"
+#define IMAGE "image"
 #define ODOM "odom"
 #define BASE_SCAN "base_scan"
 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
@@ -59,6 +61,7 @@
 {
   private:
     // Messages that we'll send or receive
+    sensor_msgs::Image *imageMsgs;
     sensor_msgs::LaserScan *laserMsgs;
     nav_msgs::Odometry *odomMsgs;
     nav_msgs::Odometry *groundTruthMsgs;
@@ -71,8 +74,10 @@
     boost::mutex msg_lock;

     // The models that we're interested in
+    std::vector<Stg::ModelCamera *> cameramodels;
     std::vector<Stg::ModelRanger *> lasermodels;
     std::vector<Stg::ModelPosition *> positionmodels;
+    std::vector<ros::Publisher> image_pubs_;
     std::vector<ros::Publisher> laser_pubs_;
     std::vector<ros::Publisher> odom_pubs_;
     std::vector<ros::Publisher> ground_truth_pubs_;
@@ -150,6 +155,8 @@
     node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
   if (dynamic_cast<Stg::ModelPosition *>(mod))
     node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod));
+  if (dynamic_cast<Stg::ModelCamera *>(mod))
+    node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
 }

 void
@@ -186,7 +193,7 @@
   Stg::Init( &argc, &argv );

   if(gui)
-    this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
+    this->world = new Stg::WorldGui(400, 300, "Stage (ROS)");
   else
     this->world = new Stg::World();

@@ -207,13 +214,21 @@
               "the world file.");
     ROS_BREAK();
   }
+  else if (cameramodels.size() > 0 && cameramodels.size() != positionmodels.size())
+  {
+    ROS_FATAL("number of position models and camera models must be equal in "
+              "the world file.");
+    ROS_BREAK();
+  }
+
   size_t numRobots = positionmodels.size();
-  ROS_INFO("found %u position/laser pair%s in the file", 
+  ROS_INFO("found %u set%s of position/laser/camera in the file", 
            (unsigned int)numRobots, (numRobots==1) ? "" : "s");

   this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
   this->odomMsgs = new nav_msgs::Odometry[numRobots];
   this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
+  this->imageMsgs = new sensor_msgs::Image[numRobots];
 }


@@ -248,9 +263,19 @@
       ROS_ERROR("no position");
       return(-1);
     }
+    if(this->cameramodels[r])
+    {
+      this->cameramodels[r]->Subscribe();
+    }
+    else
+    {
+      ROS_ERROR("no camera");
+      return(-1);
+    }
     laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
     odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
     ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
+    image_pubs_.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE,r), 10));
     cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
   }
   clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
@@ -262,6 +287,7 @@
   delete[] laserMsgs;
   delete[] odomMsgs;
   delete[] groundTruthMsgs;
+  delete[] imageMsgs;
 }

 bool
@@ -345,6 +371,22 @@
                                           mapName("base_footprint", r),
                                           mapName("base_link", r)));

+    // Get latest image data
+    // Translate into ROS message format and publish
+    if (this->cameramodels[r]->FrameColor()) {
+   this->imageMsgs[r].height=this->cameramodels[r]->getHeight();
+   this->imageMsgs[r].width=this->cameramodels[r]->getWidth();
+   this->imageMsgs[r].encoding="rgba8";
+   //this->imageMsgs[r].is_bigendian="";
+   this->imageMsgs[r].step=this->imageMsgs[r].width*4;
+   this->imageMsgs[r].data.resize(this->imageMsgs[r].width*this->imageMsgs[r].height*4);
+   memcpy(&(this->imageMsgs[r].data[0]),this->cameramodels[r]->FrameColor(),this->imageMsgs[r].width*this->imageMsgs[r].height*4);
+        this->imageMsgs[r].header.frame_id = mapName("image", r);
+        this->imageMsgs[r].header.stamp = sim_time;
+
+        this->image_pubs_[r].publish(this->imageMsgs[r]);
+    }
+
     // Get latest odometry data
     // Translate into ROS message format and publish
     this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
Index: world/willow-erratic.world
===================================================================
--- world/willow-erratic.world  (revision 40069)
+++ world/willow-erratic.world  (working copy)
@@ -1,3 +1,4 @@
+
 define block model
 (
   size [0.5 0.5 0.5]
@@ -25,6 +26,21 @@
   gui_nose 1
   drive "diff"
   topurg(pose [ 0.050 0.000 0 0.000 ])
+
+  camera
+  (
+    # laser properties
+    resolution [ 32 32 ]
+    range [ 0.2 8.0 ]
+    fov [ 70.0 40.0 ]
+    pantilt [ 0.0 0.0 ]
+
+    # model properties
+    size [ 0.1 0.07 0.05 ]
+    color "black"
+    watts 100.0 # TODO find watts for sony pan-tilt camera
+  )
+
 )

 define floorplan model
click to hide/show revision 2
Updated the url for the video

code.ros is having certificate issues, so until that gets resolved I'm sharing this here. There are a couple of things that I wish I had more time to address, these include using the image_transport, inverting the image, addressing the matter of whether a camera can exist with out a host model.

As always:

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

The proposed changes are as follows:

Index: src/stageros.cpp
===================================================================
--- src/stageros.cpp    (revision 40069)
+++ src/stageros.cpp    (working copy)
@@ -42,6 +42,7 @@
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/thread.hpp>
 #include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/Image.h>
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/Twist.h>
 #include <rosgraph_msgs/Clock.h>
@@ -49,6 +50,7 @@
 #include "tf/transform_broadcaster.h"

 #define USAGE "stageros <worldfile>"
+#define IMAGE "image"
 #define ODOM "odom"
 #define BASE_SCAN "base_scan"
 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
@@ -59,6 +61,7 @@
 {
   private:
     // Messages that we'll send or receive
+    sensor_msgs::Image *imageMsgs;
     sensor_msgs::LaserScan *laserMsgs;
     nav_msgs::Odometry *odomMsgs;
     nav_msgs::Odometry *groundTruthMsgs;
@@ -71,8 +74,10 @@
     boost::mutex msg_lock;

     // The models that we're interested in
+    std::vector<Stg::ModelCamera *> cameramodels;
     std::vector<Stg::ModelRanger *> lasermodels;
     std::vector<Stg::ModelPosition *> positionmodels;
+    std::vector<ros::Publisher> image_pubs_;
     std::vector<ros::Publisher> laser_pubs_;
     std::vector<ros::Publisher> odom_pubs_;
     std::vector<ros::Publisher> ground_truth_pubs_;
@@ -150,6 +155,8 @@
     node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
   if (dynamic_cast<Stg::ModelPosition *>(mod))
     node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod));
+  if (dynamic_cast<Stg::ModelCamera *>(mod))
+    node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
 }

 void
@@ -186,7 +193,7 @@
   Stg::Init( &argc, &argv );

   if(gui)
-    this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
+    this->world = new Stg::WorldGui(400, 300, "Stage (ROS)");
   else
     this->world = new Stg::World();

@@ -207,13 +214,21 @@
               "the world file.");
     ROS_BREAK();
   }
+  else if (cameramodels.size() > 0 && cameramodels.size() != positionmodels.size())
+  {
+    ROS_FATAL("number of position models and camera models must be equal in "
+              "the world file.");
+    ROS_BREAK();
+  }
+
   size_t numRobots = positionmodels.size();
-  ROS_INFO("found %u position/laser pair%s in the file", 
+  ROS_INFO("found %u set%s of position/laser/camera in the file", 
            (unsigned int)numRobots, (numRobots==1) ? "" : "s");

   this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
   this->odomMsgs = new nav_msgs::Odometry[numRobots];
   this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
+  this->imageMsgs = new sensor_msgs::Image[numRobots];
 }


@@ -248,9 +263,19 @@
       ROS_ERROR("no position");
       return(-1);
     }
+    if(this->cameramodels[r])
+    {
+      this->cameramodels[r]->Subscribe();
+    }
+    else
+    {
+      ROS_ERROR("no camera");
+      return(-1);
+    }
     laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
     odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
     ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
+    image_pubs_.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE,r), 10));
     cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
   }
   clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
@@ -262,6 +287,7 @@
   delete[] laserMsgs;
   delete[] odomMsgs;
   delete[] groundTruthMsgs;
+  delete[] imageMsgs;
 }

 bool
@@ -345,6 +371,22 @@
                                           mapName("base_footprint", r),
                                           mapName("base_link", r)));

+    // Get latest image data
+    // Translate into ROS message format and publish
+    if (this->cameramodels[r]->FrameColor()) {
+   this->imageMsgs[r].height=this->cameramodels[r]->getHeight();
+   this->imageMsgs[r].width=this->cameramodels[r]->getWidth();
+   this->imageMsgs[r].encoding="rgba8";
+   //this->imageMsgs[r].is_bigendian="";
+   this->imageMsgs[r].step=this->imageMsgs[r].width*4;
+   this->imageMsgs[r].data.resize(this->imageMsgs[r].width*this->imageMsgs[r].height*4);
+   memcpy(&(this->imageMsgs[r].data[0]),this->cameramodels[r]->FrameColor(),this->imageMsgs[r].width*this->imageMsgs[r].height*4);
+        this->imageMsgs[r].header.frame_id = mapName("image", r);
+        this->imageMsgs[r].header.stamp = sim_time;
+
+        this->image_pubs_[r].publish(this->imageMsgs[r]);
+    }
+
     // Get latest odometry data
     // Translate into ROS message format and publish
     this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
Index: world/willow-erratic.world
===================================================================
--- world/willow-erratic.world  (revision 40069)
+++ world/willow-erratic.world  (working copy)
@@ -1,3 +1,4 @@
+
 define block model
 (
   size [0.5 0.5 0.5]
@@ -25,6 +26,21 @@
   gui_nose 1
   drive "diff"
   topurg(pose [ 0.050 0.000 0 0.000 ])
+
+  camera
+  (
+    # laser properties
+    resolution [ 32 32 ]
+    range [ 0.2 8.0 ]
+    fov [ 70.0 40.0 ]
+    pantilt [ 0.0 0.0 ]
+
+    # model properties
+    size [ 0.1 0.07 0.05 ]
+    color "black"
+    watts 100.0 # TODO find watts for sony pan-tilt camera
+  )
+
 )

 define floorplan model

code.rosFinally submitted the ticket is having certificate issues, so until that gets resolved I'm sharing this here. There are . Updated the patch a couple of things that I wish I bit.. had more some time to address, these include using fix the image_transport, inverting the image, addressing the matter of whether a camera can exist with out a host model.inversion issue..

As always:

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

The proposed changes are as follows:

Index: src/stageros.cpp
===================================================================
--- src/stageros.cpp    (revision 40069)
+++ src/stageros.cpp    (working copy)
@@ -42,6 +42,7 @@
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/thread.hpp>
 #include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/Image.h>
 #include <nav_msgs/Odometry.h>
 #include <geometry_msgs/Twist.h>
 #include <rosgraph_msgs/Clock.h>
@@ -49,6 +50,7 @@
 #include "tf/transform_broadcaster.h"

 #define USAGE "stageros <worldfile>"
+#define IMAGE "image"
 #define ODOM "odom"
 #define BASE_SCAN "base_scan"
 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
@@ -59,6 +61,7 @@
 {
   private:
     // Messages that we'll send or receive
+    sensor_msgs::Image *imageMsgs;
     sensor_msgs::LaserScan *laserMsgs;
     nav_msgs::Odometry *odomMsgs;
     nav_msgs::Odometry *groundTruthMsgs;
@@ -71,8 +74,10 @@
     boost::mutex msg_lock;

     // The models that we're interested in
+    std::vector<Stg::ModelCamera *> cameramodels;
     std::vector<Stg::ModelRanger *> lasermodels;
     std::vector<Stg::ModelPosition *> positionmodels;
+    std::vector<ros::Publisher> image_pubs_;
     std::vector<ros::Publisher> laser_pubs_;
     std::vector<ros::Publisher> odom_pubs_;
     std::vector<ros::Publisher> ground_truth_pubs_;
@@ -150,6 +155,8 @@
     node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
   if (dynamic_cast<Stg::ModelPosition *>(mod))
     node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod));
+  if (dynamic_cast<Stg::ModelCamera *>(mod))
+    node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
 }

 void
@@ -186,7 +193,7 @@
   Stg::Init( &argc, &argv );

   if(gui)
-    this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
+    this->world = new Stg::WorldGui(400, 300, "Stage (ROS)");
   else
     this->world = new Stg::World();

@@ -207,13 +214,21 @@
               "the world file.");
     ROS_BREAK();
   }
+  else if (cameramodels.size() > 0 && cameramodels.size() != positionmodels.size())
+  {
+    ROS_FATAL("number of position models and camera models must be equal in "
+              "the world file.");
+    ROS_BREAK();
+  }
+
   size_t numRobots = positionmodels.size();
-  ROS_INFO("found %u position/laser pair%s in the file", 
+  ROS_INFO("found %u set%s of position/laser/camera in the file", 
            (unsigned int)numRobots, (numRobots==1) ? "" : "s");

   this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
   this->odomMsgs = new nav_msgs::Odometry[numRobots];
   this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
+  this->imageMsgs = new sensor_msgs::Image[numRobots];
 }


@@ -248,9 +263,19 @@
       ROS_ERROR("no position");
       return(-1);
     }
+    if(this->cameramodels[r])
+    {
+      this->cameramodels[r]->Subscribe();
+    }
+    else
+    {
+      ROS_ERROR("no camera");
+      return(-1);
+    }
     laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
     odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
     ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
+    image_pubs_.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE,r), 10));
     cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
   }
   clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
@@ -262,6 +287,7 @@
   delete[] laserMsgs;
   delete[] odomMsgs;
   delete[] groundTruthMsgs;
+  delete[] imageMsgs;
 }

 bool
@@ -345,6 +371,22 @@
                                           mapName("base_footprint", r),
                                           mapName("base_link", r)));

+    // Get latest image data
+    // Translate into ROS message format and publish
+    if (this->cameramodels[r]->FrameColor()) {
+   this->imageMsgs[r].height=this->cameramodels[r]->getHeight();
+   this->imageMsgs[r].width=this->cameramodels[r]->getWidth();
+   this->imageMsgs[r].encoding="rgba8";
+   //this->imageMsgs[r].is_bigendian="";
+   this->imageMsgs[r].step=this->imageMsgs[r].width*4;
+   this->imageMsgs[r].data.resize(this->imageMsgs[r].width*this->imageMsgs[r].height*4);
+   memcpy(&(this->imageMsgs[r].data[0]),this->cameramodels[r]->FrameColor(),this->imageMsgs[r].width*this->imageMsgs[r].height*4);
+        this->imageMsgs[r].header.frame_id = mapName("image", r);
+        this->imageMsgs[r].header.stamp = sim_time;
+
+        this->image_pubs_[r].publish(this->imageMsgs[r]);
+    }
+
     // Get latest odometry data
     // Translate into ROS message format and publish
     this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
Index: world/willow-erratic.world
===================================================================
--- world/willow-erratic.world  (revision 40069)
+++ world/willow-erratic.world  (working copy)
@@ -1,3 +1,4 @@
+
 define block model
 (
   size [0.5 0.5 0.5]
@@ -25,6 +26,21 @@
   gui_nose 1
   drive "diff"
   topurg(pose [ 0.050 0.000 0 0.000 ])
+
+  camera
+  (
+    # laser properties
+    resolution [ 32 32 ]
+    range [ 0.2 8.0 ]
+    fov [ 70.0 40.0 ]
+    pantilt [ 0.0 0.0 ]
+
+    # model properties
+    size [ 0.1 0.07 0.05 ]
+    color "black"
+    watts 100.0 # TODO find watts for sony pan-tilt camera
+  )
+
 )

 define floorplan model