31 #include <sys/types.h> 43 #include <boost/thread/mutex.hpp> 44 #include <boost/thread/thread.hpp> 45 #include <sensor_msgs/LaserScan.h> 46 #include <sensor_msgs/Image.h> 48 #include <sensor_msgs/CameraInfo.h> 49 #include <nav_msgs/Odometry.h> 50 #include <geometry_msgs/Twist.h> 51 #include <rosgraph_msgs/Clock.h> 53 #include <std_srvs/Empty.h> 57 #define USAGE "stageros <worldfile>" 60 #define CAMERA_INFO "camera_info" 62 #define BASE_SCAN "base_scan" 63 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth" 64 #define CMD_VEL "cmd_vel" 127 const char *
mapName(
const char *name,
size_t robotID, Stg::Model* mod)
const;
128 const char *
mapName(
const char *name,
size_t robotID,
size_t deviceID, Stg::Model* mod)
const;
147 StageNode(
int argc,
char** argv,
bool gui,
const char* fname,
bool use_model_names);
166 bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
181 static char buf[100];
182 std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(
":");
184 if ((found==std::string::npos) && umn)
186 snprintf(buf,
sizeof(buf),
"/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
190 snprintf(buf,
sizeof(buf),
"/robot_%u/%s", (
unsigned int)robotID, name);
207 static char buf[100];
208 std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(
":");
210 if ((found==std::string::npos) && umn)
212 snprintf(buf,
sizeof(buf),
"/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (
unsigned int)deviceID);
216 snprintf(buf,
sizeof(buf),
"/robot_%u/%s_%u", (
unsigned int)robotID, name, (
unsigned int)deviceID);
223 static char buf[100];
224 snprintf(buf,
sizeof(buf),
"/%s_%u", name, (
unsigned int)deviceID);
234 if (dynamic_cast<Stg::ModelRanger *>(mod)) {
235 node->
lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
237 if (dynamic_cast<Stg::ModelPosition *>(mod)) {
238 Stg::ModelPosition * p =
dynamic_cast<Stg::ModelPosition *
>(mod);
243 if (dynamic_cast<Stg::ModelCamera *>(mod)) {
244 node->
cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
267 boost::mutex::scoped_lock lock(
msg_lock);
281 if(!localn.
getParam(
"base_watchdog_timeout", t))
292 if(stat(fname, &s) != 0)
294 ROS_FATAL(
"The world file %s does not exist.", fname);
299 Stg::Init( &argc, &argv );
302 this->
world =
new Stg::WorldGui(600, 400,
"Stage (ROS)");
304 this->
world =
new Stg::World();
306 this->
world->Load(fname);
310 this->
world->AddUpdateCallback((Stg::world_callback_t)
s_update,
this);
313 this->
world->ForEachDescendant((Stg::model_callback_t)
ghfunc,
this);
358 ROS_INFO(
"Robot %s provided %lu rangers and %lu cameras",
411 return this->
world->UpdateAll();
418 ROS_INFO(
"ros::ok() is false. Quitting." );
419 this->
world->QuitAll();
423 boost::mutex::scoped_lock lock(
msg_lock);
428 if(this->
sim_time.
sec == 0 && this->sim_time.nsec == 0)
430 ROS_DEBUG(
"Skipping initial simulation step, to avoid publishing clock==0");
450 Stg::ModelRanger
const* lasermodel = robotmodel->
lasermodels[
s];
451 const std::vector<Stg::ModelRanger::Sensor>& sensors = lasermodel->GetSensors();
453 if( sensors.size() > 1 )
454 ROS_WARN(
"ROS Stage currently supports rangers with 1 sensor only." );
458 const Stg::ModelRanger::Sensor& sensor = sensors[0];
460 if( sensor.ranges.size() )
463 sensor_msgs::LaserScan msg;
464 msg.angle_min = -sensor.fov/2.0;
465 msg.angle_max = +sensor.fov/2.0;
466 msg.angle_increment = sensor.fov/(double)(sensor.sample_count-1);
467 msg.range_min = sensor.range.min;
468 msg.range_max = sensor.range.max;
469 msg.ranges.resize(sensor.ranges.size());
470 msg.intensities.resize(sensor.intensities.size());
472 for(
unsigned int i = 0; i < sensor.ranges.size(); i++)
474 msg.ranges[i] = sensor.ranges[i];
475 msg.intensities[i] = sensor.intensities[i];
479 msg.header.frame_id =
mapName(
"base_laser_link", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
481 msg.header.frame_id =
mapName(
"base_laser_link", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
489 Stg::Pose lp = lasermodel->GetPose();
491 laserQ.
setRPY(0.0, 0.0, lp.a);
512 nav_msgs::Odometry odom_msg;
513 odom_msg.pose.pose.position.x = robotmodel->
positionmodel->est_pose.x;
514 odom_msg.pose.pose.position.y = robotmodel->
positionmodel->est_pose.y;
517 odom_msg.twist.twist.linear.x = v.x;
518 odom_msg.twist.twist.linear.y = v.y;
519 odom_msg.twist.twist.angular.z = v.a;
524 odom_msg.header.frame_id =
mapName(
"odom", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
532 tf::Transform txOdom(odomQ,
tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
538 Stg::Pose gpose = robotmodel->
positionmodel->GetGlobalPose();
540 q_gpose.
setRPY(0.0, 0.0, gpose.a);
543 Stg::Velocity gvel(0,0,0,0);
548 gvel = Stg::Velocity(
549 (gpose.x - prevpose.x)/dT,
550 (gpose.y - prevpose.y)/dT,
551 (gpose.z - prevpose.z)/dT,
552 Stg::normalize(gpose.a - prevpose.a)/dT
558 nav_msgs::Odometry ground_truth_msg;
559 ground_truth_msg.pose.pose.position.x = gt.
getOrigin().
x();
560 ground_truth_msg.pose.pose.position.y = gt.
getOrigin().
y();
561 ground_truth_msg.pose.pose.position.z = gt.
getOrigin().
z();
562 ground_truth_msg.pose.pose.orientation.x = gt.
getRotation().x();
563 ground_truth_msg.pose.pose.orientation.y = gt.
getRotation().y();
564 ground_truth_msg.pose.pose.orientation.z = gt.
getRotation().z();
565 ground_truth_msg.pose.pose.orientation.w = gt.
getRotation().w();
566 ground_truth_msg.twist.twist.linear.x = gvel.x;
567 ground_truth_msg.twist.twist.linear.y = gvel.y;
568 ground_truth_msg.twist.twist.linear.z = gvel.z;
569 ground_truth_msg.twist.twist.angular.z = gvel.a;
571 ground_truth_msg.header.frame_id =
mapName(
"odom", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
572 ground_truth_msg.header.stamp =
sim_time;
582 if (robotmodel->
image_pubs[
s].getNumSubscribers() > 0 && cameramodel->FrameColor())
584 sensor_msgs::Image image_msg;
586 image_msg.height = cameramodel->getHeight();
587 image_msg.width = cameramodel->getWidth();
588 image_msg.encoding =
"rgba8";
590 image_msg.step = image_msg.width*4;
591 image_msg.data.resize(image_msg.width * image_msg.height * 4);
593 memcpy(&(image_msg.data[0]), cameramodel->FrameColor(), image_msg.width * image_msg.height * 4);
596 int height = image_msg.height - 1;
597 int linewidth = image_msg.width*4;
599 char* temp =
new char[linewidth];
600 for (
int y = 0;
y < (height+1)/2;
y++)
602 memcpy(temp,&image_msg.data[
y*linewidth],linewidth);
603 memcpy(&(image_msg.data[
y*linewidth]),&(image_msg.data[(height-
y)*linewidth]),linewidth);
604 memcpy(&(image_msg.data[(height-
y)*linewidth]),temp,linewidth);
608 image_msg.header.frame_id =
mapName(
"camera", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
610 image_msg.header.frame_id =
mapName(
"camera", r,static_cast<Stg::Model*>(robotmodel->
positionmodel));
619 if (robotmodel->
depth_pubs[
s].getNumSubscribers()>0 && cameramodel->FrameDepth())
621 sensor_msgs::Image depth_msg;
622 depth_msg.height = cameramodel->getHeight();
623 depth_msg.width = cameramodel->getWidth();
627 size_t len = depth_msg.width * depth_msg.height;
628 depth_msg.step = depth_msg.width * sz;
629 depth_msg.data.resize(len*sz);
633 double nearClip = cameramodel->getCamera().nearClip();
634 double farClip = cameramodel->getCamera().farClip();
635 memcpy(&(depth_msg.data[0]),cameramodel->FrameDepth(),len*sz);
636 float * data = (
float*)&(depth_msg.data[0]);
637 for (
size_t i=0;i<len;++i)
638 if(data[i]<=nearClip)
640 else if(data[i]>=farClip)
644 int nearClip = (int)(cameramodel->getCamera().nearClip() * 1000);
645 int farClip = (int)(cameramodel->getCamera().farClip() * 1000);
646 for (
size_t i=0;i<len;++i){
647 int v = (int)(cameramodel->FrameDepth()[i]*1000);
648 if (v<=nearClip || v>=farClip) v = 0;
649 ((uint16_t*)&(depth_msg.data[0]))[i] = (uint16_t) ((v<=nearClip || v>=farClip) ? 0 : v );
654 int height = depth_msg.height - 1;
655 int linewidth = depth_msg.width*sz;
657 char* temp =
new char[linewidth];
658 for (
int y = 0;
y < (height+1)/2;
y++)
660 memcpy(temp,&depth_msg.data[
y*linewidth],linewidth);
661 memcpy(&(depth_msg.data[
y*linewidth]),&(depth_msg.data[(height-
y)*linewidth]),linewidth);
662 memcpy(&(depth_msg.data[(height-
y)*linewidth]),temp,linewidth);
666 depth_msg.header.frame_id =
mapName(
"camera", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
668 depth_msg.header.frame_id =
mapName(
"camera", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
674 if ((robotmodel->
image_pubs[
s].getNumSubscribers()>0 && cameramodel->FrameColor())
675 || (robotmodel->
depth_pubs[
s].getNumSubscribers()>0 && cameramodel->FrameDepth()))
678 Stg::Pose lp = cameramodel->GetPose();
680 (cameramodel->getCamera().pitch()*M_PI/180.0)-M_PI,
682 lp.a+(cameramodel->getCamera().yaw()*M_PI/180.0) - robotmodel->
positionmodel->GetPose().a
696 sensor_msgs::CameraInfo camera_msg;
698 camera_msg.header.frame_id =
mapName(
"camera", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
700 camera_msg.header.frame_id =
mapName(
"camera", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
702 camera_msg.height = cameramodel->getHeight();
703 camera_msg.width = cameramodel->getWidth();
706 cx = camera_msg.width / 2.0;
707 cy = camera_msg.height / 2.0;
708 double fovh = cameramodel->getCamera().horizFov()*M_PI/180.0;
709 double fovv = cameramodel->getCamera().vertFov()*M_PI/180.0;
712 fx = cameramodel->getWidth()/(2*tan(fovh/2));
713 fy = cameramodel->getHeight()/(2*tan(fovv/2));
718 camera_msg.D.resize(4, 0.0);
720 camera_msg.K[0] = fx;
721 camera_msg.K[2] = cx;
722 camera_msg.K[4] = fy;
723 camera_msg.K[5] = cy;
724 camera_msg.K[8] = 1.0;
726 camera_msg.R[0] = 1.0;
727 camera_msg.R[4] = 1.0;
728 camera_msg.R[8] = 1.0;
730 camera_msg.P[0] = fx;
731 camera_msg.P[2] = cx;
732 camera_msg.P[5] = fy;
733 camera_msg.P[6] = cy;
734 camera_msg.P[10] = 1.0;
744 rosgraph_msgs::Clock clock_msg;
762 for(
int i=0;i<(argc-1);i++)
764 if(!strcmp(argv[i],
"-g"))
766 if(!strcmp(argv[i],
"-u"))
767 use_model_names =
true;
770 StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names);
775 boost::thread t = boost::thread(boost::bind(&
ros::spin));
#define BASE_POSE_GROUND_TRUTH
bool cb_reset_srv(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
const char * mapName(const char *name, size_t robotID, Stg::Model *mod) const
void publish(const boost::shared_ptr< M > &message) const
Stg::ModelPosition * positionmodel
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::vector< ros::Publisher > camera_pubs
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cmdvelReceived(int idx, const boost::shared_ptr< geometry_msgs::Twist const > &msg)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::vector< StageRobot const * > robotmodels_
ROSCPP_DECL void spin(Spinner &spinner)
std::vector< ros::Publisher > laser_pubs
TFSIMD_FORCE_INLINE const tfScalar & x() const
static bool s_update(Stg::World *world, StageNode *node)
std::vector< Stg::ModelRanger * > lasermodels
TFSIMD_FORCE_INLINE const tfScalar & z() const
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Duration & fromSec(double t)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
int main(int argc, char **argv)
std::vector< Stg::ModelPosition * > positionmodels
const std::string TYPE_32FC1
TFSIMD_FORCE_INLINE const tfScalar & y() const
const std::string TYPE_16UC1
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher clock_pub_
ros::Subscriber cmdvel_sub
std::vector< Stg::Pose > initial_poses
ros::Publisher ground_truth_pub
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
ros::Time base_last_globalpos_time
static void ghfunc(Stg::Model *mod, StageNode *node)
ros::ServiceServer reset_srv_
bool getParam(const std::string &key, std::string &s) const
std::vector< Stg::ModelCamera * > cameramodels
ros::Duration base_watchdog_timeout
StageNode(int argc, char **argv, bool gui, const char *fname, bool use_model_names)
std::vector< ros::Publisher > depth_pubs
std::vector< ros::Publisher > image_pubs
std::vector< Stg::ModelCamera * > cameramodels
tf::TransformBroadcaster tf
std::vector< Stg::ModelRanger * > lasermodels
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
std::vector< Stg::Pose > base_last_globalpos