Advertisement
shankerkeshavdas

ROS-Answers/Navigation/NavGoal

Feb 1st, 2012
845
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #ifndef TAROT_RobotControl_NavigateRobot_CPP_
  2. #define TAROT_RobotControl_NavigateRobot_CPP_
  3.  
  4. #include <ros/ros.h>
  5. #include <geometry_msgs/PoseStamped.h>
  6. #include <tf/transform_datatypes.h>
  7.  
  8. // Send a simple navigation goal
  9.  
  10. geometry_msgs::PoseStamped constructPoseStampedMsg(float xPos, float yPos, float angle)
  11. {
  12. geometry_msgs::PoseStamped poseMsg;
  13. poseMsg.header.frame_id = "/map";
  14. poseMsg.header.stamp = ros::Time::now();
  15. poseMsg.pose.position.x = xPos;
  16. poseMsg.pose.position.y = yPos;
  17. poseMsg.pose.position.z = 0;
  18. tf::Quaternion quat = tf::createQuaternionFromYaw(angle);
  19. tf::quaternionTFToMsg(quat, poseMsg.pose.orientation);
  20. return poseMsg;
  21. }
  22.  
  23. void publishNavGoal(ros::Publisher pub, geometry_msgs::PoseStamped poseMsg)
  24. {
  25. ros::Rate loop_rate(1);
  26. for(int ctr=0;ctr<2;ctr++)
  27. {
  28. pub.publish(poseMsg);
  29. ros::spinOnce();
  30. loop_rate.sleep();
  31. }
  32. return;
  33. }
  34.  
  35. int main(int argc, char **argv)
  36. {
  37. ros::init(argc, argv, "Tarot_NavigateRobot");
  38. ros::NodeHandle node;
  39. ros::Publisher pub = node.advertise<geometry_msgs::PoseStamped> ("/move_base_simple/goal",30);
  40. geometry_msgs::PoseStamped moveShortAhead = constructPoseStampedMsg(0.5,0,0);
  41. publishNavGoal(pub, moveShortAhead);
  42. return 1;
  43. }
  44.  
  45.  
  46. #endif
Advertisement
Advertisement
Advertisement
RAW Paste Data Copied
Advertisement