ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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
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
3 | No.3 Revision |
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