Skip to content

Commit

Permalink
Added ability to programatically change robot pose
Browse files Browse the repository at this point in the history
  • Loading branch information
kleinma committed Sep 24, 2018
1 parent ffed6ca commit cfeb05c
Showing 1 changed file with 19 additions and 0 deletions.
19 changes: 19 additions & 0 deletions src/stageros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <rosgraph_msgs/Clock.h>
#include <geometry_msgs/Pose2D.h>

#include <std_srvs/Empty.h>

Expand All @@ -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
Expand Down Expand Up @@ -96,6 +98,7 @@ class StageNode
std::vector<ros::Publisher> laser_pubs; //multiple lasers

ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
ros::Subscriber pose_sub; //one pos subscriber
};

std::vector<StageRobot const *> robotmodels_;
Expand Down Expand Up @@ -161,6 +164,9 @@ class StageNode
// Message callback for a MsgBaseVel message, which set velocities.
void cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg);

// Message callback for a Pose2D message, which sets pose.
void poseReceived(int idx, const boost::shared_ptr<geometry_msgs::Pose2D const>& msg);

// Service callback for soft reset
bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

Expand Down Expand Up @@ -266,6 +272,18 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist
this->base_last_cmd = this->sim_time;
}

void
StageNode::poseReceived(int idx, const boost::shared_ptr<geometry_msgs::Pose2D const>& 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;
Expand Down Expand Up @@ -354,6 +372,7 @@ StageNode::SubscribeModels()
new_robot->odom_pub = n_.advertise<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
new_robot->cmdvel_sub = n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));
new_robot->pose_sub = n_.subscribe<geometry_msgs::Pose2D>(mapName(POSE, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::poseReceived, this, r, _1));

for (size_t s = 0; s < new_robot->lasermodels.size(); ++s)
{
Expand Down

0 comments on commit cfeb05c

Please sign in to comment.