ROS机器人从起点到终点（四）蓝桥云实践复现

可否在蓝桥ROS中复现呢？试一试吧：

首先需要下载已经打包的案例：

需要修改源码使用wget下载到code文件夹：

使用其中的turtlesim包：

需要修改CMakelist和添加move.cpp

1☞move

代码语言：javascript

复制

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"
#include "math.h"
#include <sstream>
ros::Subscriber sub;

ros::Publisher pub;

float goal_x = 2;

float goal_y = 2;
void sendVel(const turtlesim::Pose::ConstPtr& data){

ros::NodeHandle n;
pub = n.advertise&lt;geometry_msgs::Twist&gt;(&#34;/turtle1/cmd_vel&#34;,100);

float curr_x = data-&gt;x;
float curr_y = data-&gt;y;
float curr_ang = data-&gt;theta;

float dist = sqrt(pow(goal_x-curr_x,2) + pow(goal_y-curr_y,2));
std::cout &lt;&lt; &#34;Distance = &#34; &lt;&lt; dist &lt;&lt; std::endl;

if(dist &gt; 0.01){
    double ang = atan2((float)(goal_y-curr_y),(float)(goal_x-curr_x));

    std::cout &lt;&lt; &#34;Curr_ang = &#34; &lt;&lt; curr_ang &lt;&lt; &#34; | ang = &#34; &lt;&lt; ang &lt;&lt; std::endl;

    geometry_msgs::Twist t_msg;

    t_msg.linear.x = 1.0*(dist);
    t_msg.angular.z = 4.0*(ang-curr_ang);

    pub.publish(t_msg);
}
else
{
	std::cout &lt;&lt; &#34;Mission Completed&#34; &lt;&lt; std::endl;
	std::cout &lt;&lt; &#34;Please enter new coordinates&#34; &lt;&lt; std::endl;
	std::cout &lt;&lt; &#34;Please enter goal_x:&#34; &lt;&lt; std::endl;
	std::cin &gt;&gt; goal_x;
	std::cout &lt;&lt; &#34;Please enter goal_y:&#34; &lt;&lt; std::endl;
	std::cin &gt;&gt; goal_y;
}

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

ros::init(argc,argv,"goToGoal");
ros::NodeHandle n;

sub = n.subscribe(&#34;/turtle1/pose&#34;,100,sendVel);

ros::spin();


return 0;

}

2☞CMakeList

编译一次试试看吧？

没有报错。

回到起点，试试看？

测试多次，稳定性ok。

代码语言：javascript

复制

shiyanlou:/ $ history                                               [20:44:44]

1  wget https://labfile.oss.aliyuncs.com/courses/854/rosdemos_ws.zip

2  unzip rosdemos_ws.zip

3  cd demo_ws

4  catkin_make

5  source devel/setup.zsh

6  roscore

7  rosrun turtlesim turtlesim_node

8  rosrun turtlesim move

shiyanlou:/ $                                                  [20:44:55]