更新robot_demo中的小车走正方形的程序

README.md

@@ -5,11 +5,11 @@
 `./update_code.sh`
 
 ## 2. 启动运行代码
-这里只需要执行如下命令就可以将整个小车启动起来，进入到基本工作状态，命令如下：   
-`roslaunch robot_bringup robot_bringup`
+这里只需要执行如下命令就可以将整个小车启动起来，进入到基本工作状态，命令如下：
+`roslaunch robot_bringup robot_bringup.launch`
 
 ## 3. 启动键盘遥控小车移动
-这里使用了teleop_twist_keyboard来遥控小车四处移动，启动命令如下：   
+这里使用了teleop_twist_keyboard来遥控小车四处移动，启动命令如下：
 `rosrun control_robot control_robot.py`
 
 ## 4. 查看摄像头画面

src/robot_demo/cfg/drive_square.yaml

@@ -1,18 +1,15 @@
 #####################################################
-#  
 # Description:测试走正方形小程序
 #   angular_speed:角速度.
 #   linear_speed:线速度.
 #   square_side_length:正方形边长.
-# Author: adam_zhuo
+# Author: adam_zhuo,corvin_zhang
 # History:
 #   20210512:init this file.
 ######################################################
-
 odom_frame: /odom_combined
 base_frame: /base_footprint
 
-square_side_length: 0.3
+square_side_length: 0.4
 angular_speed: 0.6
 linear_speed: 0.2
-

src/robot_demo/launch/drive_square.launch

@@ -1,16 +1,19 @@
 <!--
- * @Author: adam_zhuo
+ * @Author: adam_zhuo,corvin_zhang
  * @Date: 2021-04-30 11:16:35
  * @Description: 启动走正方形节点
  * @History: 20210430:初始化文件-adam_zhuo
+ *  20220810:在启动小车走正方形节点前,先将robot_bringup.launch启动;
 -->
 <launch>
-
-  <!-- 加载参数 -->
+  <!-- load param -->
   <arg name="cfg_file" default="$(find robot_demo)/cfg/drive_square.yaml" />
 
-  <!-- 启动drive_square_node -->
+  <!-- launch robot_bringup.launch -->
+  <include file="$(find robot_bringup)/launch/robot_bringup.launch" />
+
+  <!-- run drive_square_node -->
   <node pkg="robot_demo" type="robot_demo_drive_square_node" name="drive_square_node" output="screen">
     <rosparam file="$(arg cfg_file)" command="load" />
   </node>
-</launch>
+</launch>

src/robot_demo/src/drive_square_node.cpp

@@ -6,12 +6,13 @@
  */
 #include "drive_square.h"
 
-Drive_Square::Drive_Square(){
+Drive_Square::Drive_Square()
+{
     ros::param::param<std::string>("~odom_frame", odom_frame, "odom");
     ros::param::param<std::string>("~base_frame", base_frame, "base_link");
     ros::param::param<float>("~angular_speed", angular_speed, 0.2);
     ros::param::param<float>("~linear_speed", linear_speed, 0.2);
-    ros::param::param<float>("~square_side_length", square_side_length, 0.2);
+    ros::param::param<float>("~square_side_length", square_side_length, 0.4);
     _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
     go();
 }
@@ -20,13 +21,15 @@ Drive_Square::~Drive_Square(){
 
 }
 
-void Drive_Square::publish_twist_cmd(float x, float z){
+void Drive_Square::publish_twist_cmd(float x, float z)
+{
     twist.linear.x = x;
     twist.angular.z = z;
     _cmd_vel_pub.publish(twist);
 }
 
-void Drive_Square::go_forward(const float square_side_length){
+void Drive_Square::go_forward(const float square_side_length)
+{
     tf::TransformListener tf_Listener;
     tf::StampedTransform tf_Transform;
     float dist = 0.0; //每次前进移动的距离
@@ -52,7 +55,7 @@ void Drive_Square::go_forward(const float square_side_length){
         publish_twist_cmd(linear_speed, 0);
         ros::Duration(0.10).sleep(); //100 ms
 
-        tf_Listener.lookupTransform(odom_frame, base_frame, ros::Time(0),tf_Transform);
+        tf_Listener.lookupTransform(odom_frame, base_frame, ros::Time(0), tf_Transform);
         float x = tf_Transform.getOrigin().x();
         float y = tf_Transform.getOrigin().y();
         dist = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
@@ -61,7 +64,8 @@ void Drive_Square::go_forward(const float square_side_length){
     ROS_INFO("Go forward finish !!!");
 }
 
-void Drive_Square::go_circle(){
+void Drive_Square::go_circle()
+{
     tf::TransformListener tf_Listener;
     tf::StampedTransform tf_Transform;
     float check_angle = M_PI / 2.0;
@@ -83,7 +87,7 @@ void Drive_Square::go_circle(){
     while((fabs(turn_angle) < check_angle)&&(ros::ok()))
     {
         publish_twist_cmd(0, angular_speed); //原地转弯,不需要linear_x,y的控制
-        ros::Duration(0.10).sleep(); //100 ms
+        ros::Duration(0.05).sleep(); //50 ms
 
         try
         {
@@ -106,7 +110,8 @@ void Drive_Square::go_circle(){
     ros::Duration(0.1).sleep(); //100 ms
 }
 
-void Drive_Square::go(){
+void Drive_Square::go()
+{
     for (size_t i = 0; i < 4; i++)
     {
         Drive_Square::go_forward(square_side_length);
@@ -124,4 +129,4 @@ int main(int argc, char **argv)
 
     //使ROS中回调函数生效
     ros::spin();
-}
+}