How to transfer numbers in messages?
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 ...