更新红外测距模块走迷宫代码

src/infrared_maze/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   nav_msgs
   tf
+  std_msgs
 )
 
 ## System dependencies are found with CMake's conventions

src/infrared_maze/cfg/param.yaml

@@ -5,13 +5,16 @@
 # Author: corvin
 # History:
 #   20200404:init this file.
+#   20200406:增加yaw_data话题,rate参数.
 ######################################################
 infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
 cmd_topic: /cmd_vel
+yaw_topic: /yaw_data
 linear_x: 0.17
 linear_y: 0.15
-angular_speed: 0.4
+angular_speed: 0.5
 warn_dist: 0.12
 tolerance_dist: 0.05
 odom_frame: /odom_combined
 base_frame: /base_footprint
+rate: 1

src/infrared_maze/src/infrared_maze.cpp

@@ -10,6 +10,7 @@
 #include <geometry_msgs/Twist.h>
 #include <tf/transform_listener.h>
 #include <nav_msgs/Odometry.h>
+#include <std_msgs/Float32.h>
 
 
 #define  TURN_LEFT    0
@@ -29,9 +30,9 @@ float right_dist;
 std::string odomFrame;
 std::string baseFrame;
 
-float linear_x_speed = 0.18;
-float linear_y_speed = 0.16;
-float angular_speed = 0.4;
+float linear_x_speed = 0.17;
+float linear_y_speed = 0.15;
+float angular_speed = 0.5;
 
 /*********************************************************
  * Descripition: 发布控制移动的消息,这里对于平面移动的
@@ -56,11 +57,11 @@ void publishTwistCmd(float linear_x, float linear_y, float angular_z)
  * Description:控制小车移动在过道中间,比较左右两边的距离后
  *   然后进行水平左右移动.
  **********************************************************/
-void horizontallyMoveMiddle(float left_dis,float right_dis,float tolerance)
+void horizMoveMiddle(float left,float right,float tolerance)
 {
-    float diff = left_dis - right_dis;
-    //ROS_INFO("left right diff:%f", diff);
-    if((fabs(diff) > tolerance)&&(left_dis != 0.300000)&&(right_dis != 0.300000))
+    float diff = left - right;
+    ROS_INFO("diff:%f,left:%f,right:%f",diff,left,right);
+    if((fabs(diff) > tolerance)&&(left!=(float)0.3)&&(right!=(float)0.3))
     {
         if(diff < 0) //move right
         {
@@ -80,6 +81,45 @@ void horizontallyMoveMiddle(float left_dis,float right_dis,float tolerance)
     }
 }
 
+/****************************************************
+ * Description:控制小车前进移动0.5m
+ ***************************************************/
+void goForward()
+{
+    tf::TransformListener tf_Listener;
+    tf::StampedTransform tf_Transform;
+    float dist = 0.0;
+    float check_dist = 0.5;
+
+    try
+    {
+        tf_Listener.waitForTransform(odomFrame, baseFrame, ros::Time(0), ros::Duration(5.0));
+    }
+    catch(tf::TransformException &ex)
+    {
+        ROS_ERROR("tf_Listener.waitForTransform timeout error !");
+        ROS_ERROR("%s",ex.what());
+        ros::shutdown();
+    }
+
+    tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+    float x_start = tf_Transform.getOrigin().x();
+    float y_start = tf_Transform.getOrigin().y();
+    while((dist < check_dist) && (ros::ok()))
+    {
+        ROS_INFO("Go forward ...");
+        publishTwistCmd(linear_x_speed, 0, 0);
+        ros::Duration(0.10).sleep(); //100 ms
+        tf_Listener.lookupTransform(odomFrame, baseFrame, 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));
+    }
+    publishTwistCmd(0, 0, 0); //stop move
+    ROS_INFO("Go forward finish !!!");
+    ros::Duration(0.3).sleep(); //300 ms
+}
+
 /**************************************************
  * Description:控制小车左右转90度,或者掉头180度.
  *************************************************/
@@ -133,7 +173,7 @@ int controlTurn(int flag)
     {
         ROS_INFO("Turning ...");
         publishTwistCmd(0, 0, angular); //原地转弯,不需要linear_x,y的控制
-        ros::Duration(0.04).sleep(); //40 ms
+        ros::Duration(0.10).sleep(); //100 ms
 
         try
         {
@@ -151,6 +191,7 @@ int controlTurn(int flag)
         last_angle = rotation;
         //ROS_INFO("turn_angle:%f",turn_angle);
     }
+    publishTwistCmd(0, 0, 0); //原地转弯,不需要linear_x,y的控制
     ROS_INFO("Turning finish !!!");
     ros::Duration(0.3).sleep(); //300 ms
 
@@ -158,53 +199,72 @@ int controlTurn(int flag)
 }
 
 /***************************************************************************
- * Description:检查三个传感器数据,然后控制小车移动.
+ * Description:检查三个传感器数据,然后控制小车移动.这里需要防止小车在原地
+ *   进行连续两次旋转,在每次转弯后必须有个直行.
  **************************************************************************/
 void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         float tolerance)
 {
-    if(infrared_f < warn_dist*2.0) //前边距离小于警告值
+    static int flag = 0;
+    static int cnt = 0;
+
+    if((infrared_l == (float)0.3)&&(!flag))
     {
-        if(infrared_l > infrared_r)
+        cnt++;
+        ROS_WARN("Ready turn Left !");
+        if(cnt >= 7)
         {
             ROS_WARN("turn Left !");
             controlTurn(TURN_LEFT);
+            flag = 1;
+            cnt = 0;
         }
-        else if(infrared_l < infrared_r)
+        if(infrared_f > tolerance*4.0)
         {
-            ROS_WARN("turn Right !");
-            controlTurn(TURN_RIGHT);
+            ROS_WARN("continue go forward !");
+            //始终控制小车保持在过道的中间位置
+            horizMoveMiddle(infrared_l, infrared_r, tolerance);
+            publishTwistCmd(linear_x_speed, 0, 0); //go forward
         }
-#if 0
-        if(fabs(infrared_l - infrared_r) > tolerance)
+    }
+    else if(infrared_f > tolerance*4.0)
+    {
+        //始终控制小车保持在过道的中间位置
+        horizMoveMiddle(infrared_l, infrared_r, tolerance);
+        publishTwistCmd(linear_x_speed, 0, 0); //go forward
+        flag = 0;
+    }
+    else if((infrared_r == (float)0.3)&&(!flag))
+    {
+        cnt++;
+        ROS_WARN("Ready turn Right !");
+        if(cnt >= 7)
         {
-            if(infrared_l > infrared_r)
-            {
-                ROS_WARN("turn Left !");
-                controlTurn(TURN_LEFT);
-            }
-            else if(infrared_l < infrared_r)
-            {
-                ROS_WARN("turn Right !");
-                controlTurn(TURN_RIGHT);
-            }
+            ROS_WARN("turn Right !");
+            controlTurn(TURN_RIGHT);
+            flag = 1;
+            cnt = 0;
         }
-        else
+        if(infrared_f > tolerance*4.0)
         {
-            ROS_WARN("turn Back !");
-            controlTurn(TURN_BACK);
+            ROS_WARN("continue go forward !");
+            //始终控制小车保持在过道的中间位置
+            horizMoveMiddle(infrared_l, infrared_r, tolerance);
+            publishTwistCmd(linear_x_speed, 0, 0); //go forward
         }
-#endif
     }
     else
     {
-       //始终控制小车保持在过道的中间位置
-        horizontallyMoveMiddle(infrared_l, infrared_r, tolerance);
-        ROS_INFO("Go forward, linear_x:%f", linear_x_speed);
-        publishTwistCmd(linear_x_speed, 0, 0);
+        ROS_WARN("turn Back !");
+        controlTurn(TURN_BACK);
     }
 }
 
+void yaw_callback(const std_msgs::Float32::ConstPtr& msg)
+{
+    ROS_INFO("Yaw data:%f", msg->data);
+}
+
 /*****************************************************************************
  * Description: 订阅红外测距传感器话题后的回调函数,根据三个传感器数据进行走
  *   迷宫.
@@ -225,6 +285,7 @@ int main(int argc, char **argv)
 {
     std::string infrared_topic;
     std::string cmd_topic;
+    std::string yaw_topic;
 
     ros::init(argc, argv, "infrared_maze_node");
     ros::NodeHandle handle;
@@ -238,10 +299,21 @@ int main(int argc, char **argv)
     ros::param::get("~tolerance_dist", tolerance_dist);
     ros::param::get("~odom_frame", odomFrame);
     ros::param::get("~base_frame", baseFrame);
+    ros::param::get("~yaw_topic",  yaw_topic);
 
     ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
+    ros::Subscriber sub_yaw = handle.subscribe(yaw_topic, 1, yaw_callback);
     twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
-
+#if 0
+    controlTurn(TURN_RIGHT);
+    ros::Duration(3).sleep(); //300 ms
+    controlTurn(TURN_RIGHT);
+    ros::Duration(3).sleep(); //300 ms
+    controlTurn(TURN_RIGHT);
+    ros::Duration(3).sleep(); //300 ms
+    controlTurn(TURN_RIGHT);
+    ros::Duration(3).sleep(); //300 ms
+#endif
     ros::spin();
     return 0;
 }

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/param.yaml

@@ -1,4 +1,14 @@
+###############################################
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:使用IIC总线来读取IMU模块的数据,并
+#   通过话题将imu数据发送出来.这里是可以配置的
+#   一些参数.
+# Author: corvin
+# History:
+#   20200406:init this file.
+################################################
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
+yaw_topic: yaw_data
 link_name: base_imu_link
 pub_hz: 10

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_node.py

@@ -11,6 +11,7 @@
 # History:
 #    20191031: Initial this file.
 #    20191209: 新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
+#    20200406: 增加可以直接发布yaw数据的话题.
 
 import rospy
 import math
@@ -40,11 +41,15 @@ data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
 link_name  = rospy.get_param('~link_name', 'base_imu_link')
 pub_hz = rospy.get_param('~pub_hz', '10')
+yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
 
 data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
 temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
+yaw_pub = rospy.Publisher(yaw_topic_name, Float32, queue_size=1)
 srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
+
 imuMsg = Imu()
+yawMsg = Float32()
 
 # Orientation covariance estimation
 imuMsg.orientation_covariance = [
@@ -90,6 +95,9 @@ while not rospy.is_shutdown():
     pitch = float(myIMU.raw_pitch)*degrees2rad
     roll  = float(myIMU.raw_roll)*degrees2rad
 
+    yawMsg.data = yaw
+    yaw_pub.publish(yawMsg)
+
     # Publish imu message
     #rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
     #              myIMU.raw_ay, myIMU.raw_az)

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/cfg/param.yaml

@@ -15,6 +15,7 @@
 # Author: corvin
 # History:
 #   20200402: init this file.
+#   20200406: 增加直接发布yaw数据的话题.
 ################################################
 imu_dev: /dev/ttyS0
 baud_rate: 9600
@@ -24,5 +25,6 @@ stop_bits: 1
 
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
+yaw_topic: yaw_data
 link_name: base_imu_link
 pub_hz: 10

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -5,11 +5,13 @@
  * Author: corvin
  * History:
  *   20200403: init this file.
+ *   20200406:增加发布yaw数据的话题.
 **************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
 #include "imu_data.h"
 #include <sensor_msgs/Imu.h>
+#include <std_msgs/Float32.h>
 
 
 int main(int argc, char **argv)
@@ -24,6 +26,7 @@ int main(int argc, char **argv)
     std::string link_name;
     std::string imu_topic;
     std::string temp_topic;
+    std::string yaw_topic;
     int pub_hz = 0;
     float degree2Rad = 3.1415926/180.0;
     float acc_factor = 9.806/256.0;
@@ -40,6 +43,7 @@ int main(int argc, char **argv)
     ros::param::get("~link_name", link_name);
     ros::param::get("~pub_data_topic", imu_topic);
     ros::param::get("~pub_temp_topic", temp_topic);
+    ros::param::get("~yaw_topic", yaw_topic);
     ros::param::get("~pub_hz", pub_hz);
 
     int ret = initSerialPort(imu_dev.c_str(), baud, dataBit, parity.c_str(), stopBit);
@@ -51,11 +55,13 @@ int main(int argc, char **argv)
     }
     ROS_INFO("IMU module is working... ");
 
-    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 5);
+    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 1);
+    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_topic, 1);
     ros::Rate loop_rate(pub_hz);
 
     sensor_msgs::Imu imu_msg;
     imu_msg.header.frame_id = link_name;
+    std_msgs::Float32 yaw_msg;
     while(ros::ok())
     {
         if(getImuData() < 0)
@@ -65,8 +71,12 @@ int main(int argc, char **argv)
         roll  = getAngleX()*degree2Rad;
         pitch = getAngleY()*degree2Rad;
         yaw   = getAngleZ()*degree2Rad;
-        if(yaw >= 180.0)
-            yaw -= 360.0;
+        if(yaw >= 3.1415926)
+            yaw -= 6.2831852;
+
+        //publish yaw data
+        yaw_msg.data = yaw;
+        yaw_pub.publish(yaw_msg);
 
         //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
         imu_msg.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);

src/robot_calibration/config/angular_calibrate_params.yaml

@@ -24,7 +24,7 @@
 #  20180425:将test_angle测试角度改为test_circle,直接指定测试几圈,
 #      将角速度从0.5rad/s改为0.2rad/s,tolerance_angel从0.03->0.05.
 #######################################################################
-test_circle: 2 #test run circle
+test_circle: 1 #test run circle
 angular_speed: 0.5  #default 0.2rad/s
 tolerance_angle: 0.04 #tolerance radians
 angular_scale: 1.00