From 11530f3c57b97d3cb523935df24b4e1b5d3a6290 Mon Sep 17 00:00:00 2001 From: Matthew Klein Date: Fri, 11 Nov 2016 15:14:37 -0500 Subject: [PATCH 1/4] Added ability to programatically change robot pose. --- src/stageros.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/stageros.cpp b/src/stageros.cpp index b6a5869..ba7e069 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 POSE "pose" // Our node class StageNode @@ -96,6 +98,7 @@ class StageNode std::vector laser_pubs; //multiple lasers ros::Subscriber cmdvel_sub; //one cmd_vel subscriber + ros::Subscriber pose_sub; //one pos subscriber }; std::vector robotmodels_; @@ -161,6 +164,9 @@ class StageNode // Message callback for a MsgBaseVel message, which set velocities. void cmdvelReceived(int idx, const boost::shared_ptr& msg); + // Message callback for a Pose2D message, which sets pose. + void poseReceived(int idx, const boost::shared_ptr& msg); + // Service callback for soft reset bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); @@ -266,6 +272,18 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptrbase_last_cmd = this->sim_time; } +void +StageNode::poseReceived(int idx, const boost::shared_ptr& msg) +{ + boost::mutex::scoped_lock lock(msg_lock); + Stg::Pose pose; + pose.x = msg->x; + pose.y = msg->y; + pose.z =0; + pose.a = msg->theta; + this->positionmodels[idx]->SetPose(pose); +} + StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names) { this->use_model_names = use_model_names; @@ -354,6 +372,7 @@ 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->pose_sub = n_.subscribe(mapName(POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1)); for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) { @@ -734,6 +753,8 @@ StageNode::WorldCallback() int main(int argc, char** argv) { + ROS_INFO_STREAM("Custom stage_ros by MAK."); + if( argc < 2 ) { puts(USAGE); From 74eb61c0be8fe63eb622444885821d6881ccec31 Mon Sep 17 00:00:00 2001 From: Matthew Klein Date: Fri, 11 Nov 2016 15:26:45 -0500 Subject: [PATCH 2/4] Changed pose to cmd_pose to show command intent. --- src/stageros.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index ba7e069..45915af 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -62,7 +62,7 @@ #define BASE_SCAN "base_scan" #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth" #define CMD_VEL "cmd_vel" -#define POSE "pose" +#define CMD_POSE "cmd_pose" // Our node class StageNode @@ -98,7 +98,7 @@ class StageNode std::vector laser_pubs; //multiple lasers ros::Subscriber cmdvel_sub; //one cmd_vel subscriber - ros::Subscriber pose_sub; //one pos subscriber + ros::Subscriber cmdpose_sub; //one pos subscriber }; std::vector robotmodels_; @@ -165,7 +165,7 @@ class StageNode void cmdvelReceived(int idx, const boost::shared_ptr& msg); // Message callback for a Pose2D message, which sets pose. - void poseReceived(int idx, const boost::shared_ptr& msg); + void cmdposeReceived(int idx, const boost::shared_ptr& msg); // Service callback for soft reset bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); @@ -273,7 +273,7 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptr& msg) +StageNode::cmdposeReceived(int idx, const boost::shared_ptr& msg) { boost::mutex::scoped_lock lock(msg_lock); Stg::Pose pose; @@ -372,7 +372,7 @@ 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->pose_sub = n_.subscribe(mapName(POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1)); + new_robot->cmdpose_sub = n_.subscribe(mapName(CMD_POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdposeReceived, this, r, _1)); for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) { @@ -753,7 +753,7 @@ StageNode::WorldCallback() int main(int argc, char** argv) { - ROS_INFO_STREAM("Custom stage_ros by MAK."); + ROS_INFO_STREAM("Command robot pose programmatically by publishing a geometry_msgs/Pose2D to /pose (single robot) or /robot_#/pose (multiple robots). -MAK"); if( argc < 2 ) { From cfeb05c04bef330e214cdbbc8527ee782c7518a7 Mon Sep 17 00:00:00 2001 From: Matthew Klein Date: Mon, 24 Sep 2018 13:37:57 -0400 Subject: [PATCH 3/4] Added ability to programatically change robot pose --- src/stageros.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/stageros.cpp b/src/stageros.cpp index b6a5869..0047f58 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 POSE "pose" // Our node class StageNode @@ -96,6 +98,7 @@ class StageNode std::vector laser_pubs; //multiple lasers ros::Subscriber cmdvel_sub; //one cmd_vel subscriber + ros::Subscriber pose_sub; //one pos subscriber }; std::vector robotmodels_; @@ -161,6 +164,9 @@ class StageNode // Message callback for a MsgBaseVel message, which set velocities. void cmdvelReceived(int idx, const boost::shared_ptr& msg); + // Message callback for a Pose2D message, which sets pose. + void poseReceived(int idx, const boost::shared_ptr& msg); + // Service callback for soft reset bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); @@ -266,6 +272,18 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptrbase_last_cmd = this->sim_time; } +void +StageNode::poseReceived(int idx, const boost::shared_ptr& msg) +{ + boost::mutex::scoped_lock lock(msg_lock); + Stg::Pose pose; + pose.x = msg->x; + pose.y = msg->y; + pose.z =0; + pose.a = msg->theta; + this->positionmodels[idx]->SetPose(pose); +} + StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names) { this->use_model_names = use_model_names; @@ -354,6 +372,7 @@ 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->pose_sub = n_.subscribe(mapName(POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1)); for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) { From f5b16b3e0af49b124bec406854f2c541b63a3c2c Mon Sep 17 00:00:00 2001 From: Matthew Klein Date: Mon, 24 Sep 2018 13:40:03 -0400 Subject: [PATCH 4/4] Changed pose to cmd_pose to show command intent --- src/stageros.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 0047f58..edc5677 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -62,7 +62,7 @@ #define BASE_SCAN "base_scan" #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth" #define CMD_VEL "cmd_vel" -#define POSE "pose" +#define CMD_POSE "cmd_pose" // Our node class StageNode @@ -98,7 +98,7 @@ class StageNode std::vector laser_pubs; //multiple lasers ros::Subscriber cmdvel_sub; //one cmd_vel subscriber - ros::Subscriber pose_sub; //one pos subscriber + ros::Subscriber cmdpose_sub; //one pos subscriber }; std::vector robotmodels_; @@ -165,7 +165,7 @@ class StageNode void cmdvelReceived(int idx, const boost::shared_ptr& msg); // Message callback for a Pose2D message, which sets pose. - void poseReceived(int idx, const boost::shared_ptr& msg); + void cmdposeReceived(int idx, const boost::shared_ptr& msg); // Service callback for soft reset bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); @@ -273,7 +273,7 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptr& msg) +StageNode::cmdposeReceived(int idx, const boost::shared_ptr& msg) { boost::mutex::scoped_lock lock(msg_lock); Stg::Pose pose; @@ -372,7 +372,7 @@ 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->pose_sub = n_.subscribe(mapName(POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1)); + new_robot->cmdpose_sub = n_.subscribe(mapName(CMD_POSE, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdposeReceived, this, r, _1)); for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) {