This was done as roscpp

- the first node is called getpose and reads from the keyboard a target pose in two dimensions (x,y,theta) and publishes to a topic of type geometry_msgs::Pose2D. The topic is called targetpose. The node runs an infinite loop, i.e., after having read and published one pose, it waits for the user to enter a new one. The location is expressed in a world frame that coincides with the starting location of the robot.

- the second node is called gotopose. It subscribes to the targetpose topic and after having received one message with the desired pose, it issues geometry_msgs::Twist commands to get to that point. To determine if the robot has reached the target location, it subscribes to the /tf topic to figure out where it is. Once it has reached the target pose, it reads a new pose from the targetpose topic and it restarts.

what i have so far:

getpose.cpp

#include

#include

#include

#include

#include

#include

using namespace std;

float x;

float y;

float theta;

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

ros::init(argc,argv,"getpose");

ros::NodeHandle nh;

ros::Publisher pub = nh.advertise("targetpose",1000);

geometry_msgs::Pose2D poseToPublish;

tf2::Quaternion q;

while (ros::ok()) {

cin >> x;

cin >> y;

cin >> theta;

poseToPublish.position.x = x;

poseToPublish.position.y = y;

poseToPublish.position.z = 0;

q.setRPY(0,0,theta);

poseToPublish.orientation = tf2::toMsg(q);

pub.publish(poseToPublish);

}

}