正文 

当前位置： 首页 > 知识 > java教程

ros1云课→23turtlesim绘制小结（数学和编程）(代码片段)

zhangrelay zhangrelay     2022-12-03     526

关键词：

ROS1云课→22机器人轨迹跟踪

这一节就有些乱了……

机器人如何走出一个正方形的轨迹呢？

这里，采用了官方例程中：

//draw_square

#include <boost/bind.hpp>
#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Empty.h>

turtlesim::PoseConstPtr g_pose;
turtlesim::Pose g_goal;

enum State

  FORWARD,
  STOP_FORWARD,
  TURN,
  STOP_TURN,
;

State g_state = FORWARD;
State g_last_state = FORWARD;
bool g_first_goal_set = false;

#define PI 3.141592

void poseCallback(const turtlesim::PoseConstPtr& pose)

  g_pose = pose;


bool hasReachedGoal()

  return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1 && fabsf(g_pose->theta - g_goal.theta) < 0.01;


bool hasStopped()

  return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;


void printGoal()

  ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);


void commandTurtle(ros::Publisher twist_pub, float linear, float angular)

  geometry_msgs::Twist twist;
  twist.linear.x = linear;
  twist.angular.z = angular;
  twist_pub.publish(twist);


void stopForward(ros::Publisher twist_pub)

  if (hasStopped())
  
    ROS_INFO("Reached goal");
    g_state = TURN;
    g_goal.x = g_pose->x;
    g_goal.y = g_pose->y;
    g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
    printGoal();
  
  else
  
    commandTurtle(twist_pub, 0, 0);
  


void stopTurn(ros::Publisher twist_pub)

  if (hasStopped())
  
    ROS_INFO("Reached goal");
    g_state = FORWARD;
    g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
    g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
    g_goal.theta = g_pose->theta;
    printGoal();
  
  else
  
    commandTurtle(twist_pub, 0, 0);
  



void forward(ros::Publisher twist_pub)

  if (hasReachedGoal())
  
    g_state = STOP_FORWARD;
    commandTurtle(twist_pub, 0, 0);
  
  else
  
    commandTurtle(twist_pub, 1.0, 0.0);
  


void turn(ros::Publisher twist_pub)

  if (hasReachedGoal())
  
    g_state = STOP_TURN;
    commandTurtle(twist_pub, 0, 0);
  
  else
  
    commandTurtle(twist_pub, 0.0, 0.4);
  


void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)

  if (!g_pose)
  
    return;
  

  if (!g_first_goal_set)
  
    g_first_goal_set = true;
    g_state = FORWARD;
    g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
    g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
    g_goal.theta = g_pose->theta;
    printGoal();
  

  if (g_state == FORWARD)
  
    forward(twist_pub);
  
  else if (g_state == STOP_FORWARD)
  
    stopForward(twist_pub);
  
  else if (g_state == TURN)
  
    turn(twist_pub);
  
  else if (g_state == STOP_TURN)
  
    stopTurn(twist_pub);
  


int main(int argc, char** argv)

  ros::init(argc, argv, "draw_square");
  ros::NodeHandle nh;
  ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback);
  ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
  ros::ServiceClient reset = nh.serviceClient<std_srvs::Empty>("reset");
  ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twist_pub));

  std_srvs::Empty empty;
  reset.call(empty);

  ros::spin();

由于原来代码中，对于到达目标的误差设定较大：

bool hasReachedGoal()

  return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1 && fabsf(g_pose->theta - g_goal.theta) < 0.01;

如果单纯提升精度，会由于控制速度是阶跃信号（控制要么为零要么是一个固定值），导致控制失效。

    commandTurtle(twist_pub, 1.0, 0.0);

这种类似bang-bang控制。

enum State

  FORWARD,
  STOP_FORWARD,
  TURN,
  STOP_TURN,
;

State g_state = FORWARD;
State g_last_state = FORWARD;
bool g_first_goal_set = false;

为何定义四种状态？

为何控制不连续？

机器人轨迹和数学之间的联系。

数学或物理上位移和速度的关系就类似为机器人运行轨迹和机器人速度控制量之间的关系。

定理：如果函数 f 在 x = a 中可导，则 f 在 x = a 中也是连续的。

每个直角∟点，显然不连续，机器人轨迹是分段的，控制自然也分段了。

改了如上的代码，实现了高精度的曲线绘制，但是依然存在误差，更不能忍受的是，一直画下去，根本停不下来啊……

那怎么办？

需要用topic/service/action  

用服务或者行动更为合适。

参考如下程序：

#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <actionlib/server/simple_action_server.h>
#include <cmath>
#include <math.h>
#include <angles/angles.h>

#include <geometry_msgs/Twist.h>
#include <turtle_actionlib/ShapeAction.h>

// This class computes the command_velocities of the turtle to draw regular polygons 
class ShapeAction

public:
  ShapeAction(std::string name) : 
    as_(nh_, name, false),
    action_name_(name)
  
    //register the goal and feeback callbacks
    as_.registerGoalCallback(boost::bind(&ShapeAction::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&ShapeAction::preemptCB, this));

    //subscribe to the data topic of interest
    sub_ = nh_.subscribe("/turtle1/pose", 1, &ShapeAction::controlCB, this);
    pub_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1);

    as_.start();
  

  ~ShapeAction(void)
  
  

  void goalCB()
  
    // accept the new goal
    turtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();
    //save the goal as private variables
    edges_ = goal.edges;
    radius_ = goal.radius;

    // reset helper variables
    interior_angle_ = ((edges_-2)*M_PI)/edges_;
    apothem_ = radius_*cos(M_PI/edges_);
    //compute the side length of the polygon
    side_len_ = apothem_ * 2* tan( M_PI/edges_);
    //store the result values
    result_.apothem = apothem_;
    result_.interior_angle = interior_angle_;
    edge_progress_ =0;
    start_edge_ = true;
  

  void preemptCB()
  
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  

  void controlCB(const turtlesim::Pose::ConstPtr& msg)
  
    // make sure that the action hasn't been canceled
    if (!as_.isActive())
      return;
  
    if (edge_progress_ < edges_)
    
      // scalar values for drive the turtle faster and straighter
      double l_scale = 6.0;
      double a_scale = 6.0;
      double error_tol = 0.00001;

      if (start_edge_)
      
        start_x_ = msg->x;
        start_y_ = msg->y;
        start_theta_ = msg->theta;
        start_edge_ = false;
      

      // compute the distance and theta error for the shape
      dis_error_ = side_len_ - fabs(sqrt((start_x_- msg->x)*(start_x_-msg->x) + (start_y_-msg->y)*(start_y_-msg->y)));
      theta_error_ = angles::normalize_angle_positive(M_PI - interior_angle_ - (msg->theta - start_theta_));
     
      if (dis_error_ > error_tol)
      
        command_.linear.x = l_scale*dis_error_;
        command_.angular.z = 0;
      
      else if (dis_error_ < error_tol && fabs(theta_error_)> error_tol)
       
        command_.linear.x = 0;
        command_.angular.z = a_scale*theta_error_;
      
      else if (dis_error_ < error_tol && fabs(theta_error_)< error_tol)
      
        command_.linear.x = 0;
        command_.angular.z = 0;
        start_edge_ = true;
        edge_progress_++;
        
      else
      
        command_.linear.x = l_scale*dis_error_;
        command_.angular.z = a_scale*theta_error_;
       
      // publish the velocity command
      pub_.publish(command_);
      
     
    else
              
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
       
  

protected:
  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<turtle_actionlib::ShapeAction> as_;
  std::string action_name_;
  double radius_, apothem_, interior_angle_, side_len_;
  double start_x_, start_y_, start_theta_;
  double dis_error_, theta_error_;
  int edges_ , edge_progress_;
  bool start_edge_;
  geometry_msgs::Twist command_;
  turtle_actionlib::ShapeResult result_;
  ros::Subscriber sub_;
  ros::Publisher pub_;
;

int main(int argc, char** argv)

  ros::init(argc, argv, "turtle_shape");

  ShapeAction shape(ros::this_node::getName());
  ros::spin();

  return 0;

这里面，对于精度设置如下：

error_tol = 0.00001; 

用这种方式实现，还有一个有点就是可以设置任意边。

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <turtle_actionlib/ShapeAction.h>

int main (int argc, char **argv)

  ros::init(argc, argv, "test_shape"); 

  // create the action client
  // true causes the client to spin it's own thread
  actionlib::SimpleActionClient<turtle_actionlib::ShapeAction> ac("turtle_shape", true); 

  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start
  ac.waitForServer(); //will wait for infinite time
 
  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action 
  turtle_actionlib::ShapeGoal goal;
  goal.edges = 5;
  goal.radius = 1.3;
  ac.sendGoal(goal);
  
  //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(40.0));

  if (finished_before_timeout)
    
      actionlib::SimpleClientGoalState state = ac.getState();
      ROS_INFO("Action finished: %s",state.toString().c_str());
    
  else  
    ROS_INFO("Action did not finish before the time out.");

  //exit
  return 0;

在如下程序加入一些程序段：

  int edges_input;
  float radius_input;
  ROS_INFO("Please input edges:");
  std::cin>>edges_input;
  ROS_INFO("Please input radius:");
  std::cin>>radius_input;
  if(edges_input>2)
    goal.edges = edges_input;
  if(radius_input>0)
    goal.radius = radius_input;

全部代码如下： 

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <turtle_actionlib/ShapeAction.h>

int main (int argc, char **argv)

  ros::init(argc, argv, "test_shape"); 

  // create the action client
  // true causes the client to spin it's own thread
  actionlib::SimpleActionClient<turtle_actionlib::ShapeAction> ac("turtle_shape", true); 

  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start
  ac.waitForServer(); //will wait for infinite time
 
  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action 
  turtle_actionlib::ShapeGoal goal;
  goal.edges = 5;
  goal.radius = 1.3;

  int edges_input;
  float radius_input;
  ROS_INFO("Please input edges:");
  std::cin>>edges_input;
  ROS_INFO("Please input radius:");
  std::cin>>radius_input;
  if(edges_input>2)
    goal.edges = edges_input;
  if(radius_input>0)
    goal.radius = radius_input;


  ac.sendGoal(goal);
  
  //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(40.0));

  if (finished_before_timeout)
    
      actionlib::SimpleClientGoalState state = ac.getState();
      ROS_INFO("Action finished: %s",state.toString().c_str());
    
  else  
    ROS_INFO("Action did not finish before the time out.");

  //exit
  return 0;

三角形：

 

九边形：

 

二十七边形： 

 

相关博文，供参考：

ROS2趣味题库之turtlesim魔幻步伐（轨迹类题型）

玫瑰线轨迹如何规划？（desmos+ROS2+turtlesim+……） 

 

ros1云课→27机器人slam小结(代码片段)

从turtlesim到stdr，从示意到仿真。stdr：ROS1云课→26机器人Gmapping等环境地图构建ROS1云课→25机器人控制配置 ROS1云课→24机器人感知配置 turtlesim：ROS1云课→23turtlesim绘制小结（数学和编程） 为什么要学习如上... 查看详情

ros1云课→27机器人slam小结(代码片段)

从turtlesim到stdr，从示意到仿真。stdr：ROS1云课→26机器人Gmapping等环境地图构建ROS1云课→25机器人控制配置 ROS1云课→24机器人感知配置 turtlesim：ROS1云课→23turtlesim绘制小结（数学和编程） 为什么要学习如上... 查看详情

ros1云课→24机器人感知配置(代码片段)

ROS1云课→23turtlesim绘制小结（数学和编程）如上案例已成往事。接着：ROS1云课→22机器人轨迹跟踪以真实和仿真机器人共性知识点展开。机器人配置包含哪些模块呢？？？各部件如何组织？导航功能包... 查看详情

ros1云课→11曲线绘制

...文字版的，曲线是图形化显示的数据。ROS2趣味题库之turtlesim魔幻步伐（轨迹类题型）_zhangrelay的博客-CSDN博客rqt工具非常多，绘图plot只是其中之一：绘制标量数据图使用ROS中现有的一些通用工具轻松地绘制标... 查看详情

ros1云课→09功能包小定制（cli命令行接口）(代码片段)

ROS1云课→08基础实践（CLI命令行接口）默认的turtlesim：开始定制化： 首先，将turtlesim源码拷贝到base_tutorials/src目录下：图标换一下：然后修改源码turtle_frame.cpp：QVector<QString>turtles;tu 查看详情

ros1云课→09功能包小定制（cli命令行接口）(代码片段)

ROS1云课→08基础实践（CLI命令行接口）默认的turtlesim：开始定制化： 首先，将turtlesim源码拷贝到base_tutorials/src目录下：图标换一下：然后修改源码turtle_frame.cpp：QVector<QString>turtles;tu 查看详情

ros1云课→28机器人代价地图配置(代码片段)

ROS1云课→27机器人SLAM小结在前面做的所有工作都成了现在项目的铺垫，而最大的乐趣也即将开始，这是赋予机器人生命的时刻。 后续学习以下内容：应用程序包开发。理解导航功能包集及其工作方式。配置所有必要... 查看详情

ros1云课→28机器人代价地图配置(代码片段)

ROS1云课→27机器人SLAM小结在前面做的所有工作都成了现在项目的铺垫，而最大的乐趣也即将开始，这是赋予机器人生命的时刻。 后续学习以下内容：应用程序包开发。理解导航功能包集及其工作方式。配置所有必要... 查看详情

ros1云课→12图像可视化(代码片段)

ROS1云课→11曲线绘制从二维曲线过渡到二维视频流，如何在蓝桥ROS中进行实践学习呢。如上是算法实践的基础，比如人脸识别，车道识别算法，怎么做，云端也没啥摄像头。使用video_stream_opencv，下面详细... 查看详情

ros1云课-1024(代码片段)

...导航部分区域覆盖部分1.基础部分参考一键配置：ROS1云课→30导航仿真演示_zhangrelay的博客-CSDN博客ROS1云课→31欢乐卷假期_zhangrelay的博客-CSDN博客 echo"-----BEGINPGPPUBLICKE 查看详情

ros1云课→02系统架构

ROS1云课→01简介和配置ROS1系统的架构主要被设计和划分成了三部分，每一部分都代表一个层级的概念：文件系统级（TheFilesystemlevel）计算图级（TheComputationGraphlevel）开源社区级（TheCommunitylevel）第... 查看详情

ros1云课→04功能包(代码片段)

ROS1云课→03工作空间先下载课程主页提及的功能包：wgethttps://labfile.oss.aliyuncs.com/courses/854/rosdemos_ws.zip解压缩：unziprosdemos_ws.zip  选择，任一个按“ROS1云课→03工作空间”中内容，进行编译，以ros_tutorials为例& 查看详情

ros1云课→04功能包(代码片段)

ROS1云课→03工作空间先下载课程主页提及的功能包：wgethttps://labfile.oss.aliyuncs.com/courses/854/rosdemos_ws.zip解压缩：unziprosdemos_ws.zip  选择，任一个按“ROS1云课→03工作空间”中内容，进行编译，以ros_tutorials为例& 查看详情

ros1云课→05消息类型

ROS1云课→04功能包消息类似各类编程语言中的变量和常量等，ROS1中消息分为：主题消息服务消息行动消息  主题消息ROS1使用了一种简化的消息类型描述语言来描述ROS节点发布的数据值。通过这样的描述语言，ROS1能... 查看详情

ros1云课→32愉快大扫除(代码片段)

ROS1云课→31欢乐卷假期案例简述：就是扫地机器人的路径规划复现一下，没了……能不能复现别人的区域覆盖算法呢？？？ROS1云课→30导航仿真演示导航机器人摇身一变扫地机器人（只有路径规划演示没有... 查看详情

ros1云课→01简介和配置(代码片段)

 ROS1云课适用于kinetic/melodic/noetic。以蓝桥ROS云课为模板重新梳理。云原生与蓝桥ROS机器人课程2017-2022_zhangrelay的博客-CSDN博客机器人课程反馈中如何面对批评和负面消息_zhangrelay的博客-CSDN博客 ROS1系统已经支持大量机器人中的传... 查看详情

ros1云课→01简介和配置(代码片段)

 ROS1云课适用于kinetic/melodic/noetic。以蓝桥ROS云课为模板重新梳理。云原生与蓝桥ROS机器人课程2017-2022_zhangrelay的博客-CSDN博客机器人课程反馈中如何面对批评和负面消息_zhangrelay的博客-CSDN博客 ROS1系统已经支持大量机器人中的传... 查看详情

ros1云课→19仿真turtlebot（stage）(代码片段)

ROS1云课→18一键配置ROS1云课→17化繁为简stdr和f1tenth依据一键配置将turtlebot仿真案例全部配置好。TurtleBot是带有开源软件的低成本个人机器人套件。TurtleBot是MeloneeWise和TullyFoote于2010年11月在WillowGarage创建的。使用TurtleBot，将... 查看详情