ROS实践----小乌龟跑8字

快刀一哥

已于 2023-03-21 16:05:52 修改

阅读量863

点赞数 2

分类专栏： ROS 文章标签： c++ 人工智能

于 2023-03-21 15:58:15 首次发布

本文链接：https://blog.csdn.net/qingyu3044/article/details/129689679

版权

ROS 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

已经成功安装ros，安装可以参考ros安装相关文章
总体思路：
在检测到小乌龟回到记录的位置点后，掉头向反方向转圈。

涉及的topic：

/turtle1/cmd_vel
/turtle1/pose

订阅方代码

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"

bool initflag = true;

float directionX = 0;
float directionY = 0;
float angle = 0;
void doPose(const turtlesim::Pose::ConstPtr &pose)
{

    if (initflag)
    {
        directionX = pose->x;
        directionY = pose->y;
        angle = pose->theta;
        initflag = false;
        ROS_INFO("turtel locaition is(%.2f,%.2f)",pose->x,pose->y);
    }
    else { 
        if ((abs(directionX - pose->x) < 0.005) && (abs(directionY - pose->y) < 0.005))
        {

            ROS_INFO("turtle go back-------------------");
            ROS_INFO("turtle pose information, x location is (%.2f,%.2f) direction is: %.2f linar speed is:%.2f,anguler speed is: %.2f",
                pose->x,pose->y,pose->theta, pose->linear_velocity,pose->angular_velocity);
            ros::NodeHandle nh1;
            int loop1 = 0;
            bool flag = nh1.getParam("backflag",loop1);
            if(flag)
            {
               
                int ang1 = 0;
                ang1 = 0 - loop1;
                nh1.setParam("backflag",ang1);
                ROS_INFO("get parameter success backflag %d -> %d",loop1,ang1);
                
            }

        }
        
    }
  
    
}
int main(int argc, char *argv[])
{
    /* code */
    ros::init(argc,argv,"pose1");
    ros::NodeHandle nh;
    //nh.setParam("backflag",1);
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    
    
    ros::spin();
    return 0;
}

发布方代码：

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"


int main(int argc, char  *argv[])
{
    /* code */
    ros::init(argc,argv,"plumbing_test1");

    ros::NodeHandle nh;
    
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    ros::Rate rate(10);

    geometry_msgs::Twist twist;
    twist.linear.x = 1.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;

    twist.angular.z = 1.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    nh.setParam("backflag",1);
    while(ros::ok())
    {
        int loop2 = 0;
        bool flag1 = nh.getParam("backflag",loop2);
        if(flag1)
        {
            twist.angular.z =loop2;
        }
        pub.publish(twist);

        rate.sleep();

        ros::spinOnce();
    }

    return 0;
}