31 #include <sys/types.h> 42 #include <boost/thread/mutex.hpp> 43 #include <boost/thread/thread.hpp> 44 #include <sensor_msgs/LaserScan.h> 45 #include <sensor_msgs/Image.h> 47 #include <sensor_msgs/CameraInfo.h> 48 #include <nav_msgs/Odometry.h> 49 #include <geometry_msgs/Twist.h> 50 #include <rosgraph_msgs/Clock.h> 52 #include <std_srvs/Empty.h> 56 #define USAGE "stageros <worldfile>" 59 #define CAMERA_INFO "camera_info" 61 #define BASE_SCAN "base_scan" 62 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth" 63 #define CMD_VEL "cmd_vel" 127 const char *
mapName(
const char *name,
size_t robotID,
size_t deviceID,
Stg::Model* mod)
const;
146 StageNode(
int argc,
char** argv,
bool gui,
const char* fname,
bool use_model_names);
165 bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
180 static char buf[100];
181 std::size_t found = std::string(((
Stg::Ancestor *) mod)->Token()).find(
":");
183 if ((found==std::string::npos) && umn)
185 snprintf(buf,
sizeof(buf),
"/%s/%s", ((
Stg::Ancestor *) mod)->Token(), name);
189 snprintf(buf,
sizeof(buf),
"/robot_%u/%s", (
unsigned int)robotID, name);
206 static char buf[100];
207 std::size_t found = std::string(((
Stg::Ancestor *) mod)->Token()).find(
":");
209 if ((found==std::string::npos) && umn)
211 snprintf(buf,
sizeof(buf),
"/%s/%s_%u", ((
Stg::Ancestor *) mod)->Token(), name, (
unsigned int)deviceID);
215 snprintf(buf,
sizeof(buf),
"/robot_%u/%s_%u", (
unsigned int)robotID, name, (
unsigned int)deviceID);
222 static char buf[100];
223 snprintf(buf,
sizeof(buf),
"/%s_%u", name, (
unsigned int)deviceID);
231 if (dynamic_cast<Stg::ModelRanger *>(mod))
232 node->
lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
233 if (dynamic_cast<Stg::ModelPosition *>(mod)) {
239 if (dynamic_cast<Stg::ModelCamera *>(mod))
240 node->
cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
262 boost::mutex::scoped_lock lock(
msg_lock);
276 if(!localn.
getParam(
"base_watchdog_timeout", t))
288 if(stat(fname, &s) != 0)
290 ROS_FATAL(
"The world file %s does not exist.", fname);
408 boost::mutex::scoped_lock lock(
msg_lock);
413 if(this->
sim_time.
sec == 0 && this->sim_time.nsec == 0)
415 ROS_DEBUG(
"Skipping initial simulation step, to avoid publishing clock==0");
436 const std::vector<Stg::ModelRanger::Sensor>& sensors = lasermodel->
GetSensors();
438 if( sensors.size() > 1 )
439 ROS_WARN(
"ROS Stage currently supports rangers with 1 sensor only." );
445 if( sensor.
ranges.size() )
448 sensor_msgs::LaserScan msg;
449 msg.angle_min = -sensor.
fov/2.0;
450 msg.angle_max = +sensor.
fov/2.0;
454 msg.ranges.resize(sensor.
ranges.size());
457 for(
unsigned int i = 0; i < sensor.
ranges.size(); i++)
459 msg.ranges[i] = sensor.
ranges[i];
464 msg.header.frame_id =
mapName(
"base_laser_link", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
466 msg.header.frame_id =
mapName(
"base_laser_link", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
497 nav_msgs::Odometry odom_msg;
502 odom_msg.twist.twist.linear.
x = v.
x;
503 odom_msg.twist.twist.linear.y = v.
y;
504 odom_msg.twist.twist.angular.z = v.
a;
509 odom_msg.header.frame_id =
mapName(
"odom", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
517 tf::Transform txOdom(odomQ,
tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
525 q_gpose.
setRPY(0.0, 0.0, gpose.
a);
534 (gpose.
x - prevpose.
x)/dT,
535 (gpose.
y - prevpose.
y)/dT,
536 (gpose.
z - prevpose.
z)/dT,
543 nav_msgs::Odometry ground_truth_msg;
544 ground_truth_msg.pose.pose.position.x = gt.
getOrigin().
x();
545 ground_truth_msg.pose.pose.position.y = gt.
getOrigin().
y();
546 ground_truth_msg.pose.pose.position.z = gt.
getOrigin().
z();
547 ground_truth_msg.pose.pose.orientation.x = gt.
getRotation().x();
548 ground_truth_msg.pose.pose.orientation.y = gt.
getRotation().y();
549 ground_truth_msg.pose.pose.orientation.z = gt.
getRotation().z();
550 ground_truth_msg.pose.pose.orientation.w = gt.
getRotation().w();
551 ground_truth_msg.twist.twist.linear.x = gvel.
x;
552 ground_truth_msg.twist.twist.linear.y = gvel.
y;
553 ground_truth_msg.twist.twist.linear.z = gvel.
z;
554 ground_truth_msg.twist.twist.angular.z = gvel.
a;
556 ground_truth_msg.header.frame_id =
mapName(
"odom", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
557 ground_truth_msg.header.stamp =
sim_time;
569 sensor_msgs::Image image_msg;
571 image_msg.height = cameramodel->
getHeight();
572 image_msg.width = cameramodel->
getWidth();
573 image_msg.encoding =
"rgba8";
575 image_msg.step = image_msg.width*4;
576 image_msg.data.resize(image_msg.width * image_msg.height * 4);
578 memcpy(&(image_msg.data[0]), cameramodel->
FrameColor(), image_msg.width * image_msg.height * 4);
581 int height = image_msg.height - 1;
582 int linewidth = image_msg.width*4;
584 char* temp =
new char[linewidth];
585 for (
int y = 0;
y < (height+1)/2;
y++)
587 memcpy(temp,&image_msg.data[
y*linewidth],linewidth);
588 memcpy(&(image_msg.data[
y*linewidth]),&(image_msg.data[(height-
y)*linewidth]),linewidth);
589 memcpy(&(image_msg.data[(height-
y)*linewidth]),temp,linewidth);
593 image_msg.header.frame_id =
mapName(
"camera", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
595 image_msg.header.frame_id =
mapName(
"camera", r,static_cast<Stg::Model*>(robotmodel->
positionmodel));
606 sensor_msgs::Image depth_msg;
607 depth_msg.height = cameramodel->
getHeight();
608 depth_msg.width = cameramodel->
getWidth();
612 size_t len = depth_msg.width * depth_msg.height;
613 depth_msg.step = depth_msg.width * sz;
614 depth_msg.data.resize(len*sz);
620 memcpy(&(depth_msg.data[0]),cameramodel->
FrameDepth(),len*sz);
621 float * data = (
float*)&(depth_msg.data[0]);
622 for (
size_t i=0;i<len;++i)
623 if(data[i]<=nearClip)
625 else if(data[i]>=farClip)
631 for (
size_t i=0;i<len;++i){
632 int v = (int)(cameramodel->
FrameDepth()[i]*1000);
633 if (v<=nearClip || v>=farClip) v = 0;
634 ((uint16_t*)&(depth_msg.data[0]))[i] = (uint16_t) ((v<=nearClip || v>=farClip) ? 0 : v );
639 int height = depth_msg.height - 1;
640 int linewidth = depth_msg.width*sz;
642 char* temp =
new char[linewidth];
643 for (
int y = 0;
y < (height+1)/2;
y++)
645 memcpy(temp,&depth_msg.data[
y*linewidth],linewidth);
646 memcpy(&(depth_msg.data[
y*linewidth]),&(depth_msg.data[(height-
y)*linewidth]),linewidth);
647 memcpy(&(depth_msg.data[(height-
y)*linewidth]),temp,linewidth);
651 depth_msg.header.frame_id =
mapName(
"camera", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
653 depth_msg.header.frame_id =
mapName(
"camera", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
681 sensor_msgs::CameraInfo camera_msg;
683 camera_msg.header.frame_id =
mapName(
"camera", r,
s, static_cast<Stg::Model*>(robotmodel->
positionmodel));
685 camera_msg.header.frame_id =
mapName(
"camera", r, static_cast<Stg::Model*>(robotmodel->
positionmodel));
687 camera_msg.height = cameramodel->
getHeight();
688 camera_msg.width = cameramodel->
getWidth();
691 cx = camera_msg.width / 2.0;
692 cy = camera_msg.height / 2.0;
697 fx = cameramodel->
getWidth()/(2*tan(fovh/2));
698 fy = cameramodel->
getHeight()/(2*tan(fovv/2));
703 camera_msg.D.resize(4, 0.0);
705 camera_msg.K[0] = fx;
706 camera_msg.K[2] = cx;
707 camera_msg.K[4] = fy;
708 camera_msg.K[5] = cy;
709 camera_msg.K[8] = 1.0;
711 camera_msg.R[0] = 1.0;
712 camera_msg.R[4] = 1.0;
713 camera_msg.R[8] = 1.0;
715 camera_msg.P[0] = fx;
716 camera_msg.P[2] = cx;
717 camera_msg.P[5] = fy;
718 camera_msg.P[6] = cy;
719 camera_msg.P[10] = 1.0;
729 rosgraph_msgs::Clock clock_msg;
747 for(
int i=0;i<(argc-1);i++)
749 if(!strcmp(argv[i],
"-g"))
751 if(!strcmp(argv[i],
"-u"))
752 use_model_names =
true;
755 StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names);
760 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
usec_t SimTimeNow(void) 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())
int getHeight(void) const
std::vector< ros::Publisher > camera_pubs
void Init(int *argc, char **argv[])
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int(* model_callback_t)(Model *mod, void *user)
double nearClip(void) const
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)
void ForEachDescendant(model_callback_t func, void *arg)
std::vector< ros::Publisher > laser_pubs
void AddUpdateCallback(world_callback_t cb, void *user)
TFSIMD_FORCE_INLINE const tfScalar & x() const
static bool s_update(Stg::World *world, StageNode *node)
double normalize(double a)
std::vector< Stg::ModelRanger * > lasermodels
TFSIMD_FORCE_INLINE const tfScalar & z() const
unsigned int sample_count
Pose GetGlobalPose() 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
const std::vector< Sensor > & GetSensors() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher clock_pub_
ros::Subscriber cmdvel_sub
double horizFov(void) const
const GLfloat * FrameDepth() const
std::vector< Stg::Pose > initial_poses
const PerspectiveCamera & getCamera(void) const
Velocity GetVelocity() const
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
WallDuration expectedCycleTime() const
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
const GLubyte * FrameColor() const
std::vector< ros::Publisher > image_pubs
std::vector< Stg::ModelCamera * > cameramodels
std::vector< meters_t > ranges
int(* world_callback_t)(World *world, void *user)
std::vector< double > intensities
double vertFov(void) const
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
double farClip(void) const
virtual void Load(const std::string &worldfile_path)