Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <ros/ros.h>
- #include <move_base_msgs/MoveBaseAction.h>
- #include <actionlib/client/simple_action_client.h>
- #include "geometry_msgs/PointStamped.h"
- #include "tf/transform_listener.h"
- typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
- int main(int argc, char** argv)
- {
- ros::init(argc, argv, "simple_navigation_goals");
- MoveBaseClient ac("move_base", true);
- while(!ac.waitForServer(ros::Duration(5.0))){
- ROS_INFO("Waiting for the move_base action server to come up");
- }
- tf::TransformListener t;
- ros::Time cTime = ros::Time::now();
- ros::Duration dur(3.0);
- if(t.waitForTransform("/base_link", "/odom", cTime, dur)){
- move_base_msgs::MoveBaseGoal goal;
- geometry_msgs::PointStamped ps;
- ps.header.frame_id = "/odom";
- ps.header.stamp = cTime;
- ps.point.x = 2700.0;
- ps.point.y = 1700.0;
- ps.point.z = 0.0;
- geometry_msgs::PointStamped pt;
- t.transformPoint("base_link", ps, pt);
- goal.target_pose.header.frame_id = "/base_link";
- goal.target_pose.header.stamp = ros::Time::now();
- goal.target_pose.pose.position.x = pt.point.x;
- goal.target_pose.pose.position.y = pt.point.y;
- goal.target_pose.pose.position.z = pt.point.z;
- goal.target_pose.pose.orientation.w = 1.0;
- ROS_INFO("Sending goal");
- ac.sendGoal(goal);
- ac.waitForResult();
- if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
- ROS_INFO("Hooray, the base moved to 2700;1700");
- else
- ROS_INFO("The base failed to move to 2700;1700");
- }
- else{
- ROS_ERROR("Transform /base_link /odom not found");
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement