ROS 小乌龟画“8”字

ROS 小乌龟画“8”字

帮“小朋友”做的作业

效果

思路

两个圆圈组成（一个顺时针、一个逆时针）

角度接近0度，转入另一个圆

代码结构

eight_motion_turtle/
├── build/
├── devel/
├── src/
    └── eight_motion_turtle_node.cpp
    ├── launch/
    │   └── eight.launch
    ├── CMakeLists.txt
    ├── package.xml

源代码

eight_motion_turtle_node.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>

ros::Publisher pub;

bool initflag = true;

float directionX = 0;
float directionY = 0;
float angle = 0;

bool inverse = false;

void move(double x, double y) {
    geometry_msgs::Twist twist;
    twist.linear.x = x;
    twist.angular.z = y;
    pub.publish(twist);
}

void poseCallback(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(0 - pose->theta) < 0.03))
    {
         // 逻辑反向
    	if(inverse == true)
    	{inverse = false;}
    	else{inverse = true;}
    }
    }
}

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

    ros::init(argc, argv, "eight_motion_turtle");
    ros::NodeHandle n;
    pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    ros::Rate rate(10);
    
    ros::Subscriber sub = n.subscribe("/turtle1/pose", 10, poseCallback);
    ros::Rate loop_rate(1);


    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;
    
    while (ros::ok()) {
        if (inverse) {
        twist.angular.z = -1.0; //反向
        }
    else {
        twist.angular.z = 1.0; //正向
    }
        
        pub.publish(twist);
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

eight.launch

<launch>
  <node name="eight_motion_turtle_node" type="eight_motion_turtle_node" pkg="eight_motion_turtle" output="screen"/>
</launch>

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(eight_motion_turtle)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
  turtlesim
)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(eight_motion_turtle_node eight_motion_turtle_node.cpp)
target_link_libraries(eight_motion_turtle_node ${catkin_LIBRARIES})

package.xml

<?xml version="1.0"?>
<package>
  <name>eight_motion_turtle</name>
  <version>0.0.0</version>
  <description>The eight motion turtle ROS package</description>

  <maintainer email="your_email@example.com">Your Name</maintainer>
  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>

  <run_depend>roscpp</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>turtlesim</run_depend>

  <export>
  </export>
</package>

指令操作

catkin_init_workspace
catkin_make

roscore
rosrun turtlesim turtlesim_node
roslaunch eight_motion_turtle eight.launch