Ask Your Question
0

How to transfer numbers in messages?

asked 2015-04-21 04:34:01 -0500

Dio Eraclea gravatar image

Hi! I have a very simple problem, but I just cannot understand how to solve it. I need to transfer three coordinates between two nodes. One node is talker, which defines the coordinates and send it to other node, which is listener. Listener waits for the coordinates, make some operations with them and send it to navigation stack, which operates the robot in stage. I have read the tutorial about simple publisher and subscriber, but there the string is used and I just do not know, how to change it to float without problems. I tried to use ros services, but they do not create a topic, while working, also I was unable to make it normal working too. It seems to me, that after receiving the message, the program stuck in a callback sub-program and the main part of the program just cannot receive the data. I can be mistaken, as my knowledge in programming is too small. Below is the code of my node that I want to make the listener. Now I enter the coordinates when the node is started, but it is not convenient. I just want to know what I should add (and where) to make this none subscribed to some topic (for example "coordinates"), and being able to receive X, Y and Theta coordinates. The part of publisher node also would be useful for me. I would like to understand the principle. Help me please!


 #include "ros/ros.h"
 #include "move_base_msgs/MoveBaseAction.h"
 #include "actionlib/client/simple_action_client.h"
 #include "tf/transform_datatypes.h"

typedef actionlib::SimpleActionClient<move_base_msgs::movebaseaction> MoveBaseClient;

using namespace std;

int main(int argc, char** argv) {

if (argc < 2) {
    ROS_ERROR("You must specify leader robot id.");
    return -1;
}

if (argc < 3) {
    ROS_ERROR("You must specify X coordinate.");
    return -1;
}

if (argc < 4) {
    ROS_ERROR("You must specify Y coordinate.");
    return -1;
}

    //ROS_INFO(argv[2]);

char *robot_id = argv[1];

ros::init(argc, argv, "test_goals");
ros::NodeHandle nh;

    double goal_x, goal_y, goal_theta;
    if (!nh.getParam("goal_x", goal_x)){
    char *x = argv[2];
    goal_x = atof(x);}
if (!nh.getParam("goal_y", goal_y)){
    char *y = argv[3];
    goal_y = atof(y);}
if (!nh.getParam("goal_theta", goal_theta))
    goal_theta = 0;


// Create the string "robot_X/move_base"
string move_base_str = "/robot_";
move_base_str += robot_id;
move_base_str += "/move_base";

// create the action client
MoveBaseClient ac(move_base_str, true);

// Wait for the action server to become available
ROS_INFO("Waiting for the move_base action server");
ac.waitForServer(ros::Duration(5));

ROS_INFO("Connected to move base server");

// Send a goal to move_base
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();

goal.target_pose.pose.position.x = goal_x;//
goal.target_pose.pose.position.y = goal_y;//

// Convert the Euler angle to quaternion
double radians = goal_theta * (M_PI/180);//
tf::Quaternion quaternion;
quaternion = tf::createQuaternionFromYaw(radians);

geometry_msgs::Quaternion qMsg;
tf::quaternionTFToMsg(quaternion, qMsg);
goal.target_pose.pose.orientation = qMsg;

ROS_INFO("Sending goal to robot no. %s: x = %f, y = %f, theta = 0", robot_id, goal_x, goal_y);
ac.sendGoal(goal);

// Wait for the action to return
ac.waitForResult();

if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Robot ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2015-04-21 11:52:23 -0500

marguedas gravatar image

updated 2015-04-21 11:56:26 -0500

Hi, I think the easiest way to do it in your case would be to send either Point, or Vector3
You can send it using:
On the talker part:

ros::Publisher vector_pub = n.advertise<geometry_msgs::Vector3>("coordinates", 1);
geometry_msgs::Vector3 vectSend;
vectSend.x = coordinates.x;
vectSend.y = coordinates.y;
vectSend.z = coordinates.theta;
vector_pub.publish(vectSend);



On the listener part:

ros::Subscriber vector_sub = n.subscribe("coordinates", 1, coordinatesCb);
void coordinatesCb(const geometry_msgs::Vector3::ConstPtr& msg){
    float goal_x, goal_y, goal_z;
    goal_x = msg->x;
    goal_y = msg->y;
    goal_z = msg->z;
    // do whatever you want with it and send to move_base

edit flag offensive delete link more

Comments

Thanks! That worked!

Dio Eraclea gravatar imageDio Eraclea ( 2015-04-23 03:03:47 -0500 )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: 2015-04-21 04:34:01 -0500

Seen: 205 times

Last updated: Apr 21 '15