Ask Your Question
2

Stageros: how to programmatically set the pose of a simulated robot

asked 2013-04-24 09:18:05 -0600

Victor_ocv2 gravatar image

Hello,

I want to know if it is possible to programmatically set the pose of a robot in stageros. Back when we used player/stage it was possible using the simulation proxy.

NOTE: my question is the same as this one but it has not been answered yet (altough the author marked it as solved - it isnt)

edit retag flag offensive close merge delete

Comments

1

it sounds promising, I will take a close look at it and I will be back here in some days, thank you!

Victor_ocv2 gravatar imageVictor_ocv2 ( 2013-04-26 05:49:17 -0600 )edit

Ok.. I'm looking for a way for doing so too... see you.

gustavo.velascoh gravatar imagegustavo.velascoh ( 2013-04-26 06:48:23 -0600 )edit
1

I have modified the stage driver to allow the reposition of the robots using a topic. Check my answer.

Victor_ocv2 gravatar imageVictor_ocv2 ( 2013-04-26 06:59:35 -0600 )edit

1 Answer

Sort by » oldest newest most voted
2

answered 2013-04-26 06:55:39 -0600

Victor_ocv2 gravatar image

updated 2013-04-26 06:56:15 -0600

I have modified the stageros.cpp file to make it subscribe to the topic "pose" which receives geometry_msgs/Pose2D and then sets the robot's pose to the desired one. The following are the lines that I had to add to stageros.cpp:

#include <geometry_msgs/Pose2D.h>

#define POSE "pose" 

std::vector<ros::Subscriber> pose_subs_;

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);
}


pose_subs_.push_back(n_.subscribe<geometry_msgs::Pose2D>(mapName(POSE,r), 10, boost::bind(&StageNode::poseReceived, this, r, _1)));
edit flag offensive delete link more

Comments

This is for one robot, isn´t it?

gustavo.velascoh gravatar imagegustavo.velascoh ( 2013-04-26 07:01:32 -0600 )edit

It is programmed exactly as the cmd_vel subscriber, so if cmd_vel subscriber is for one robot, this one aswell

Victor_ocv2 gravatar imageVictor_ocv2 ( 2013-04-26 07:11:24 -0600 )edit

I adapted your code slightly and submitted a pull request. It works well for multiple robots, just as cmd_vel. The one change I made was to call the topic cmd_pose to signify intent (write vs. read). The pull request can be found here: https://github.com/ros-simulation/sta...

kleinma gravatar imagekleinma ( 2016-11-11 14:44:30 -0600 )edit

Whoops. It looks like a pull request already exists for this. https://github.com/ros-simulation/sta... The work is the add_pose_sub branch of the main repository.

kleinma gravatar imagekleinma ( 2016-11-11 14:56:07 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-04-24 09:18:05 -0600

Seen: 934 times

Last updated: Apr 26 '13