我是靠谱客的博主 细心发卡,这篇文章主要介绍navigation发送导航目标点,读取机器人全局位姿,现在分享给大家,希望可以做个参考。

复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#include <ros/ros.h> #include <actionlib/client/simple_action_client.h> #include <move_base_msgs/MoveBaseAction.h> #include "tf/tf.h" #include <tf/transform_listener.h> #include <geometry_msgs/PoseArray.h> typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; class MyNode { public: MoveBaseClient* ac_; double last_x_set = 0; double last_y_set = 0; double last_th_set = 0; void doStuff(const double& x_set, const double& y_set, const double& th_set) { //MoveBaseClient ac("move_base", true); ac_ = new MoveBaseClient("move_base",true); //wait for the action server to come up while(!ac_->waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the move_base action server to come up"); } move_base_msgs::MoveBaseGoal goal; if(x_set != last_x_set || y_set != last_y_set || th_set != last_th_set) { //we'll send a goal to the robot to move 1 meter forward goal.target_pose.header.frame_id = "base_link"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = x_set; goal.target_pose.pose.position.y = y_set; goal.target_pose.pose.position.z = 0; tf::Quaternion q; q.setRotation(tf::Vector3(0, 0, 1), th_set); goal.target_pose.pose.orientation.x = q.x(); goal.target_pose.pose.orientation.y = q.y(); goal.target_pose.pose.orientation.z = q.z(); goal.target_pose.pose.orientation.w = q.w(); ROS_INFO("Sending goal"); // ac.sendGoal(goal); ac_->sendGoal(goal, boost::bind(&MyNode::doneCb, this, _1, _2), MoveBaseClient::SimpleActiveCallback(), MoveBaseClient::SimpleFeedbackCallback()); } last_x_set = x_set; last_y_set = y_set; last_th_set = th_set; } void doneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result) { ROS_INFO("RESULT %s",state.toString().c_str()); // boost::unique_lock<boost::mutex> lock(wp_mutex_); switch (state.state_) { case actionlib::SimpleClientGoalState::ABORTED: { ROS_INFO("NavigationManager::moveBaseResultCallback: ABORTED"); } break; case actionlib::SimpleClientGoalState::SUCCEEDED: { ROS_INFO("NavigationManager::moveBaseResultCallback: SUCCEEDED"); } break; default: break; } } void pose_get(void) { if(listener_.waitForTransform("/map","/base_link",ros::Time(0),ros::Duration(0.2))) { listener_.lookupTransform("/map","/base_link",ros::Time(0),world_pose); geometry_msgs::Pose pose; pose.position.x = world_pose.getOrigin().getX(); pose.position.y = world_pose.getOrigin().getY(); pose.position.z = world_pose.getOrigin().getZ(); pose.orientation.x = world_pose.getRotation().getX(); pose.orientation.y = world_pose.getRotation().getY(); pose.orientation.z = world_pose.getRotation().getZ(); pose.orientation.w = world_pose.getRotation().getW(); ROS_INFO("position.x : %f",pose.position.x); ROS_INFO("position.y : %f",pose.position.y); ROS_INFO("position.z : %f",pose.position.z); ROS_INFO("orientation.x : %f",pose.orientation.x); ROS_INFO("orientation.y : %f",pose.orientation.y); ROS_INFO("orientation.z : %f",pose.orientation.z); ROS_INFO("orientation.w : %f",pose.orientation.w); } } private: tf::StampedTransform world_pose; tf::TransformListener listener_; }; int main (int argc, char **argv) { ros::init(argc, argv, "test_fibonacci_class_client"); MyNode my_node; //ros::Rate loop_rate(10); while(ros::ok()) { my_node.doStuff(1,0,1); my_node.pose_get(); // ros::spin(); ros::spinOnce(); // loop_rate.sleep(); } return 0; }

连续发目标点

复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#include <ros/ros.h> #include <actionlib/client/simple_action_client.h> #include <move_base_msgs/MoveBaseAction.h> #include "tf/tf.h" #include <tf/transform_listener.h> #include <geometry_msgs/PoseArray.h> typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; class MyNode { public: MoveBaseClient* ac_; void init(){ //MoveBaseClient ac("move_base", true); ac_ = new MoveBaseClient("move_base",true); //wait for the action server to come up while(!ac_->waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the move_base action server to come up"); } } bool doStuff(const double& x_set, const double& y_set,const double& z_set, const double& x, const double& y, const double& z, const double& w) { move_base_msgs::MoveBaseGoal goal; if(x_set != last_x_set || y_set != last_y_set) { //we'll send a goal to the robot to move 1 meter forward goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); goal.target_pose.pose.position.x = x_set; goal.target_pose.pose.position.y = y_set; goal.target_pose.pose.position.z = 0; // tf::Quaternion q; // q.setRotation(tf::Vector3(0, 0, 1), th_set); goal.target_pose.pose.orientation.x = x; goal.target_pose.pose.orientation.y = y; goal.target_pose.pose.orientation.z = z; goal.target_pose.pose.orientation.w = w; ROS_INFO("Sending goal"); // ac.sendGoal(goal); ac_->sendGoal(goal, boost::bind(&MyNode::doneCb, this, _1, _2), MoveBaseClient::SimpleActiveCallback(), MoveBaseClient::SimpleFeedbackCallback()); success_ = false; } last_x_set = x_set; last_y_set = y_set; return success_; } void doneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result) { ROS_INFO("RESULT %s",state.toString().c_str()); // boost::unique_lock<boost::mutex> lock(wp_mutex_); switch (state.state_) { case actionlib::SimpleClientGoalState::ABORTED: { ROS_INFO("NavigationManager::moveBaseResultCallback: ABORTED"); } break; case actionlib::SimpleClientGoalState::SUCCEEDED: { ROS_INFO("NavigationManager::moveBaseResultCallback: SUCCEEDED"); success_ = true; } break; default: break; } } private: tf::StampedTransform world_pose; tf::TransformListener listener_; double last_x_set ; double last_y_set ; bool success_ = false; }; int main (int argc, char **argv) { ros::init(argc, argv, "test_fibonacci_class_client"); MyNode my_node; //ros::Rate loop_rate(10); double pt[10][7] = { {0.850,-10.398,0,0,0,0.795,0.606}, {1.733,-2.025,0,0,0,0.113,0.994}, {1.733,-1.492,0,0,0,0.989,-0.148}, {-0.917,-2.046,0,0,0,0.995,-0.100}, {-0.897,-1.530,0,0,0,0.115,0.993}, {1.249,-1.192,0,0,0,0.077,0.997}, {-1.371,-0.459,0,0,0,0.065,0.998}, {1.38,-0.023,0,0,0,0.065,0.998}, {1.330,0.501,0,0,0,0.996,-0.086}, {-1.966,0.059,0,0,0,0.977,-0.082} }; my_node.init(); int ind = 0; my_node.doStuff(pt[ind][0],pt[ind][1],pt[ind][2], pt[ind][3],pt[ind][4],pt[ind][5],pt[ind][6]); // my_node.pose_get(); while(ros::ok()) { bool ok = my_node.doStuff(pt[ind][0],pt[ind][1],pt[ind][2], pt[ind][3],pt[ind][4],pt[ind][5],pt[ind][6]); // my_node.pose_get(); if(ok){ ind++; if(ind > 9){ break; } } ros::spinOnce(); // loop_rate.sleep(); } return 0; }

最后

以上就是细心发卡最近收集整理的关于navigation发送导航目标点,读取机器人全局位姿的全部内容,更多相关navigation发送导航目标点内容请搜索靠谱客的其他文章。

本图文内容来源于网友提供,作为学习参考使用,或来自网络收集整理,版权属于原作者所有。
点赞(104)

评论列表共有 0 条评论

立即
投稿
返回
顶部