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

Posted 2022-04-23 zhangrelay

关于：ROS机器人从起点到终点（三）完成

---

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

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

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

使用其中的turtlesim包：

需要修改CMakelist和添加move.cpp

1☞move

#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<geometry_msgs::Twist>("/turtle1/cmd_vel",100);
 
    float curr_x = data->x;
    float curr_y = data->y;
    float curr_ang = data->theta;
 
    float dist = sqrt(pow(goal_x-curr_x,2) + pow(goal_y-curr_y,2));
    std::cout << "Distance = " << dist << std::endl;
 
    if(dist > 0.01)
        double ang = atan2((float)(goal_y-curr_y),(float)(goal_x-curr_x));
 
        std::cout << "Curr_ang = " << curr_ang << " | ang = " << ang << 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 << "Mission Completed" << std::endl;
    	std::cout << "Please enter new coordinates" << std::endl;
    	std::cout << "Please enter goal_x:" << std::endl;
    	std::cin >> goal_x;
    	std::cout << "Please enter goal_y:" << std::endl;
    	std::cin >> goal_y;
    
 

 
int main(int argc, char **argv)
    ros::init(argc,argv,"goToGoal");
 
    ros::NodeHandle n;
 
    sub = n.subscribe("/turtle1/pose",100,sendVel);
 
    ros::spin();
 
 
    return 0;

2☞CMakeList

编译一次试试看吧？

 

没有报错。

---

回到起点，试试看？

测试多次，稳定性ok。

---

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]

---