ROS机器人turtlesim圆形轨迹的示例代码

以下是ROS机器人turtlesim圆形轨迹的示例代码：

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

const double PI = 3.14159265359;

class Turtle
{
public:
  Turtle();
  void move();

private:
  ros::NodeHandle nh_;
  ros::Publisher vel_pub_;
  ros::Subscriber pose_sub_;
  turtlesim::Pose pose_;
  double linear_vel_;
  double angular_vel_;
  double radius_;
  bool clockwise_;
};

Turtle::Turtle()
{
  vel_pub_ = nh_.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
  pose_sub_ = nh_.subscribe("turtle1/pose", 1, &Turtle::poseCallback, this);
  linear_vel_ = 1.;
  angular_vel_ = 1.;
  radius_ = 2.;
  clockwise_ = false;
}

void Turtle::move()
{
  geometry_msgs::Twist vel_msg;
  vel_msg.linear.x = linear_vel_;
  if (clockwise_)
  {
    vel_msg.angular.z = -angular_vel_;
  }
  else
  {
    vel_msg.angular.z = angular_vel_;
  }
  vel_pub_.publish(vel_msg);
}

void Turtle::poseCallback(const turtlesim::Pose::ConstPtr& pose_msg)
{
  pose_ = *pose_msg;
  double distance = sqrt(pow(pose_.x - 5.5, 2) + pow(pose_.y - 5.5, 2));
  if (distance > radius_)
  {
    clockwise_ = !clockwise_;
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "turtle_circle");
  Turtle turtle;
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    turtle.move();
    ros::spinOnce();
    loop_rate.sleep();
  }
  return ;
}