• ROS1云课→23turtlesim绘制小结（数学和编程）

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

    这一节就有些乱了……

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

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

    1. //draw_square
    2.  
    3. #include 
    4. #include 
    5. #include 
    6. #include 
    7. #include 
    8.  
    9. turtlesim::PoseConstPtr g_pose;
    10. turtlesim::Pose g_goal;
    11.  
    12. enum State
    13. {
    14.   FORWARD,
    15.   STOP_FORWARD,
    16.   TURN,
    17.   STOP_TURN,
    18. };
    19.  
    20. State g_state = FORWARD;
    21. State g_last_state = FORWARD;
    22. bool g_first_goal_set = false;
    23.  
    24. #define PI 3.141592
    25.  
    26. void poseCallback(const turtlesim::PoseConstPtr& pose)
    27. {
    28.   g_pose = pose;
    29. }
    30.  
    31. bool hasReachedGoal()
    32. {
    33.   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;
    34. }
    35.  
    36. bool hasStopped()
    37. {
    38.   return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;
    39. }
    40.  
    41. void printGoal()
    42. {
    43.   ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
    44. }
    45.  
    46. void commandTurtle(ros::Publisher twist_pub, float linear, float angular)
    47. {
    48.   geometry_msgs::Twist twist;
    49.   twist.linear.x = linear;
    50.   twist.angular.z = angular;
    51.   twist_pub.publish(twist);
    52. }
    53.  
    54. void stopForward(ros::Publisher twist_pub)
    55. {
    56.   if (hasStopped())
    57.   {
    58.     ROS_INFO("Reached goal");
    59.     g_state = TURN;
    60.     g_goal.x = g_pose->x;
    61.     g_goal.y = g_pose->y;
    62.     g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
    63.     printGoal();
    64.   }
    65.   else
    66.   {
    67.     commandTurtle(twist_pub, 0, 0);
    68.   }
    69. }
    70.  
    71. void stopTurn(ros::Publisher twist_pub)
    72. {
    73.   if (hasStopped())
    74.   {
    75.     ROS_INFO("Reached goal");
    76.     g_state = FORWARD;
    77.     g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
    78.     g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
    79.     g_goal.theta = g_pose->theta;
    80.     printGoal();
    81.   }
    82.   else
    83.   {
    84.     commandTurtle(twist_pub, 0, 0);
    85.   }
    86. }
    87.  
    88.  
    89. void forward(ros::Publisher twist_pub)
    90. {
    91.   if (hasReachedGoal())
    92.   {
    93.     g_state = STOP_FORWARD;
    94.     commandTurtle(twist_pub, 0, 0);
    95.   }
    96.   else
    97.   {
    98.     commandTurtle(twist_pub, 1.0, 0.0);
    99.   }
    100. }
    101.  
    102. void turn(ros::Publisher twist_pub)
    103. {
    104.   if (hasReachedGoal())
    105.   {
    106.     g_state = STOP_TURN;
    107.     commandTurtle(twist_pub, 0, 0);
    108.   }
    109.   else
    110.   {
    111.     commandTurtle(twist_pub, 0.0, 0.4);
    112.   }
    113. }
    114.  
    115. void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)
    116. {
    117.   if (!g_pose)
    118.   {
    119.     return;
    120.   }
    121.  
    122.   if (!g_first_goal_set)
    123.   {
    124.     g_first_goal_set = true;
    125.     g_state = FORWARD;
    126.     g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
    127.     g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
    128.     g_goal.theta = g_pose->theta;
    129.     printGoal();
    130.   }
    131.  
    132.   if (g_state == FORWARD)
    133.   {
    134.     forward(twist_pub);
    135.   }
    136.   else if (g_state == STOP_FORWARD)
    137.   {
    138.     stopForward(twist_pub);
    139.   }
    140.   else if (g_state == TURN)
    141.   {
    142.     turn(twist_pub);
    143.   }
    144.   else if (g_state == STOP_TURN)
    145.   {
    146.     stopTurn(twist_pub);
    147.   }
    148. }
    149.  
    150. int main(int argc, char** argv)
    151. {
    152.   ros::init(argc, argv, "draw_square");
    153.   ros::NodeHandle nh;
    154.   ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback);
    155.   ros::Publisher twist_pub = nh.advertise("turtle1/cmd_vel", 1);
    156.   ros::ServiceClient reset = nh.serviceClient("reset");
    157.   ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twist_pub));
    158.  
    159.   std_srvs::Empty empty;
    160.   reset.call(empty);
    161.  
    162.   ros::spin();
    163. }

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

    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  

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

    参考如下程序：

    1. #include 
    2. #include 
    3. #include 
    4. #include 
    5. #include 
    6. #include 
    7.  
    8. #include 
    9. #include 
    10.  
    11. // This class computes the command_velocities of the turtle to draw regular polygons 
    12. class ShapeAction
    13. {
    14. public:
    15.   ShapeAction(std::string name) : 
    16.     as_(nh_, name, false),
    17.     action_name_(name)
    18.   {
    19.     //register the goal and feeback callbacks
    20.     as_.registerGoalCallback(boost::bind(&ShapeAction::goalCB, this));
    21.     as_.registerPreemptCallback(boost::bind(&ShapeAction::preemptCB, this));
    22.  
    23.     //subscribe to the data topic of interest
    24.     sub_ = nh_.subscribe("/turtle1/pose", 1, &ShapeAction::controlCB, this);
    25.     pub_ = nh_.advertise("/turtle1/cmd_vel", 1);
    26.  
    27.     as_.start();
    28.   }
    29.  
    30.   ~ShapeAction(void)
    31.   {
    32.   }
    33.  
    34.   void goalCB()
    35.   {
    36.     // accept the new goal
    37.     turtle_actionlib::ShapeGoal goal = *as_.acceptNewGoal();
    38.     //save the goal as private variables
    39.     edges_ = goal.edges;
    40.     radius_ = goal.radius;
    41.  
    42.     // reset helper variables
    43.     interior_angle_ = ((edges_-2)*M_PI)/edges_;
    44.     apothem_ = radius_*cos(M_PI/edges_);
    45.     //compute the side length of the polygon
    46.     side_len_ = apothem_ * 2* tan( M_PI/edges_);
    47.     //store the result values
    48.     result_.apothem = apothem_;
    49.     result_.interior_angle = interior_angle_;
    50.     edge_progress_ =0;
    51.     start_edge_ = true;
    52.   }
    53.  
    54.   void preemptCB()
    55.   {
    56.     ROS_INFO("%s: Preempted", action_name_.c_str());
    57.     // set the action state to preempted
    58.     as_.setPreempted();
    59.   }
    60.  
    61.   void controlCB(const turtlesim::Pose::ConstPtr& msg)
    62.   {
    63.     // make sure that the action hasn't been canceled
    64.     if (!as_.isActive())
    65.       return;
    66.   
    67.     if (edge_progress_ < edges_)
    68.     {
    69.       // scalar values for drive the turtle faster and straighter
    70.       double l_scale = 6.0;
    71.       double a_scale = 6.0;
    72.       double error_tol = 0.00001;
    73.  
    74.       if (start_edge_)
    75.       {
    76.         start_x_ = msg->x;
    77.         start_y_ = msg->y;
    78.         start_theta_ = msg->theta;
    79.         start_edge_ = false;
    80.       }
    81.  
    82.       // compute the distance and theta error for the shape
    83.       dis_error_ = side_len_ - fabs(sqrt((start_x_- msg->x)*(start_x_-msg->x) + (start_y_-msg->y)*(start_y_-msg->y)));
    84.       theta_error_ = angles::normalize_angle_positive(M_PI - interior_angle_ - (msg->theta - start_theta_));
    85.      
    86.       if (dis_error_ > error_tol)
    87.       {
    88.         command_.linear.x = l_scale*dis_error_;
    89.         command_.angular.z = 0;
    90.       }
    91.       else if (dis_error_ < error_tol && fabs(theta_error_)> error_tol)
    92.       { 
    93.         command_.linear.x = 0;
    94.         command_.angular.z = a_scale*theta_error_;
    95.       }
    96.       else if (dis_error_ < error_tol && fabs(theta_error_)< error_tol)
    97.       {
    98.         command_.linear.x = 0;
    99.         command_.angular.z = 0;
    100.         start_edge_ = true;
    101.         edge_progress_++;
    102.       }  
    103.       else
    104.       {
    105.         command_.linear.x = l_scale*dis_error_;
    106.         command_.angular.z = a_scale*theta_error_;
    107.       } 
    108.       // publish the velocity command
    109.       pub_.publish(command_);
    110.       
    111.     } 
    112.     else
    113.     {          
    114.       ROS_INFO("%s: Succeeded", action_name_.c_str());
    115.       // set the action state to succeeded
    116.       as_.setSucceeded(result_);
    117.     }   
    118.   }
    119.  
    120. protected:
    121.   ros::NodeHandle nh_;
    122.   actionlib::SimpleActionServer as_;
    123.   std::string action_name_;
    124.   double radius_, apothem_, interior_angle_, side_len_;
    125.   double start_x_, start_y_, start_theta_;
    126.   double dis_error_, theta_error_;
    127.   int edges_ , edge_progress_;
    128.   bool start_edge_;
    129.   geometry_msgs::Twist command_;
    130.   turtle_actionlib::ShapeResult result_;
    131.   ros::Subscriber sub_;
    132.   ros::Publisher pub_;
    133. };
    134.  
    135. int main(int argc, char** argv)
    136. {
    137.   ros::init(argc, argv, "turtle_shape");
    138.  
    139.   ShapeAction shape(ros::this_node::getName());
    140.   ros::spin();
    141.  
    142.   return 0;
    143. }

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

    error_tol = 0.00001; 

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

    1. #include 
    2. #include 
    3. #include 
    4. #include 
    5.  
    6. int main (int argc, char **argv)
    7. {
    8.   ros::init(argc, argv, "test_shape"); 
    9.  
    10.   // create the action client
    11.   // true causes the client to spin it's own thread
    12.   actionlib::SimpleActionClient ac("turtle_shape", true); 
    13.  
    14.   ROS_INFO("Waiting for action server to start.");
    15.   // wait for the action server to start
    16.   ac.waitForServer(); //will wait for infinite time
    17.  
    18.   ROS_INFO("Action server started, sending goal.");
    19.   // send a goal to the action 
    20.   turtle_actionlib::ShapeGoal goal;
    21.   goal.edges = 5;
    22.   goal.radius = 1.3;
    23.   ac.sendGoal(goal);
    24.   
    25.   //wait for the action to return
    26.   bool finished_before_timeout = ac.waitForResult(ros::Duration(40.0));
    27.  
    28.   if (finished_before_timeout)
    29.     {
    30.       actionlib::SimpleClientGoalState state = ac.getState();
    31.       ROS_INFO("Action finished: %s",state.toString().c_str());
    32.     }
    33.   else  
    34.     ROS_INFO("Action did not finish before the time out.");
    35.  
    36.   //exit
    37.   return 0;
    38. }

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

      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;

    全部代码如下： 

    1. #include 
    2. #include 
    3. #include 
    4. #include 
    5.  
    6. int main (int argc, char **argv)
    7. {
    8.   ros::init(argc, argv, "test_shape"); 
    9.  
    10.   // create the action client
    11.   // true causes the client to spin it's own thread
    12.   actionlib::SimpleActionClient ac("turtle_shape", true); 
    13.  
    14.   ROS_INFO("Waiting for action server to start.");
    15.   // wait for the action server to start
    16.   ac.waitForServer(); //will wait for infinite time
    17.  
    18.   ROS_INFO("Action server started, sending goal.");
    19.   // send a goal to the action 
    20.   turtle_actionlib::ShapeGoal goal;
    21.   goal.edges = 5;
    22.   goal.radius = 1.3;
    23.  
    24.   int edges_input;
    25.   float radius_input;
    26.   ROS_INFO("Please input edges:");
    27.   std::cin>>edges_input;
    28.   ROS_INFO("Please input radius:");
    29.   std::cin>>radius_input;
    30.   if(edges_input>2)
    31.     goal.edges = edges_input;
    32.   if(radius_input>0)
    33.     goal.radius = radius_input;
    34.  
    35.  
    36.   ac.sendGoal(goal);
    37.   
    38.   //wait for the action to return
    39.   bool finished_before_timeout = ac.waitForResult(ros::Duration(40.0));
    40.  
    41.   if (finished_before_timeout)
    42.     {
    43.       actionlib::SimpleClientGoalState state = ac.getState();
    44.       ROS_INFO("Action finished: %s",state.toString().c_str());
    45.     }
    46.   else  
    47.     ROS_INFO("Action did not finish before the time out.");
    48.  
    49.   //exit
    50.   return 0;
    51. }

    三角形：

     

    九边形：

     

    二十七边形： 

     

    相关博文，供参考：

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

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

     

  • 相关阅读:
    1688关键字搜索商品
    乐观模式下分库分表合并迁移
    混淆矩阵绘制
    根据身份证号回填信息
    AutoDL使用手册
    Web基础与HTTP协议
    JavaScript中的深拷贝和浅拷贝
    你知道HTTP与HTTPS有什么区别吗？
    如何在linux系统中设置定时任务？
    Dubbo启动报错
  • 原文地址：https://blog.csdn.net/ZhangRelay/article/details/126789498