diff --git a/src/stageros.cpp b/src/stageros.cpp index b6a5869..4e5d2f6 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -61,6 +62,7 @@ #define BASE_SCAN "base_scan" #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth" #define CMD_VEL "cmd_vel" +#define STALLED "stalled" // Our node class StageNode @@ -89,6 +91,7 @@ class StageNode //ros publishers ros::Publisher odom_pub; //one odom ros::Publisher ground_truth_pub; //one ground truth + ros::Publisher stall_pub; // stall publisher std::vector image_pubs; //multiple images std::vector depth_pubs; //multiple depths @@ -354,7 +357,8 @@ StageNode::SubscribeModels() new_robot->odom_pub = n_.advertise(mapName(ODOM, r, static_cast(new_robot->positionmodel)), 10); new_robot->ground_truth_pub = n_.advertise(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast(new_robot->positionmodel)), 10); new_robot->cmdvel_sub = n_.subscribe(mapName(CMD_VEL, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)); - + new_robot->stall_pub = n_.advertise(mapName(STALLED, r, static_cast(new_robot->positionmodel)), 10); + for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) { if (new_robot->lasermodels.size() == 1) @@ -503,13 +507,15 @@ StageNode::WorldCallback() odom_msg.twist.twist.linear.y = v.y; odom_msg.twist.twist.angular.z = v.a; - //@todo Publish stall on a separate topic when one becomes available - //this->odomMsgs[r].stall = this->positionmodels[r]->Stall(); - // odom_msg.header.frame_id = mapName("odom", r, static_cast(robotmodel->positionmodel)); odom_msg.header.stamp = sim_time; - + robotmodel->odom_pub.publish(odom_msg); + + //publish current stall state + std_msgs::Int8 stall_msg; + stall_msg.data = robotmodel->positionmodel->Stalled(); + robotmodel->stall_pub.publish(stall_msg); // broadcast odometry transform tf::Quaternion odomQ;