增加小车走正方形代码

src/robot_demo/src/drive_square.cpp

@@ -0,0 +1,369 @@
+/***************************************************************
+ Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ Description:控制两轮小车走一个正方形,正方形的边长可以通过配置
+ 　　文件来配置.
+ Author: corvin
+ History:
+    20210717:init this code.
+ **************************************************************/
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_listener.h>
+#include "rasp_imu_hat_6dof/GetYawData.h"
+
+
+#define  GO_FORWARD   0
+#define  TURN_LEFT    1
+#define  TURN_RIGHT   2
+#define  TURN_BACK    3
+#define  HORIZ_MOVE   4
+
+#define  ERROR       -1
+
+//global variable
+ros::Publisher twist_pub;
+ros::ServiceClient yawSrvClient;
+rasp_imu_hat_6dof::GetYawData yawSrv;
+
+//红外测距相关的服务
+ros::ServiceClient distSrvClient;
+ros_arduino_msgs::GetInfraredDistance distSrv;
+
+float warn_dist = 0.25;  //warn check distance
+float tolerance_dist = 0.05; //对警告距离的容忍值
+float forward_dist = 0.50; //每次前进移动时走的距离
+
+std::string odomFrame;
+std::string baseFrame;
+
+float linear_x_speed = 0.27;
+float angular_speed = 1.0;
+
+float start_yaw_data = 0.0;
+int turnFlag = ERROR;
+
+/*********************************************************
+ * Descripition: 发布控制移动的消息,这里对于平面移动的
+ *   机器人只需要控制linear_x和angular_z即可.
+ ********************************************************/
+void publishTwistCmd(float linear_x, float angular_z)
+{
+    geometry_msgs::Twist twist_msg;
+
+    twist_msg.linear.x = linear_x;
+    twist_msg.linear.y = 0.0;
+    twist_msg.linear.z = 0.0;
+
+    twist_msg.angular.x = 0.0;
+    twist_msg.angular.y = 0.0;
+    twist_msg.angular.z = angular_z;
+
+    twist_pub.publish(twist_msg);
+}
+
+/**********************************************************
+ * Description:调用IMU板发布出来的yaw信息,可以根据该数据
+ *   来修正转弯时不准确的问题.注意这里的yaw数据为弧度值.
+ *   注意往右旋转为负值,左旋转为正值.右手定则.
+ *   注意在ROS中IMU数据中yaw的范围,具体表示如下:
+ *                0  -0
+ *                -----
+ *              /      \
+ *         1.57|        | -1.57
+ *              \      /
+ *               ------
+ *             3.14 -3.14
+ **********************************************************/
+void yawUpdate(const float start_yaw)
+{
+    float target = 0.0;
+    float diff = 10.0;
+    float angular = 0.0;
+    float now_yaw = 0.0;
+
+    //开始计算修正后的目标角度
+    if((turnFlag == GO_FORWARD)||(turnFlag == HORIZ_MOVE))
+    {
+        target = start_yaw;
+    }
+    else if(turnFlag == TURN_LEFT)
+    {
+        target = start_yaw + M_PI/2.0;
+        if(target > M_PI)
+        {
+            target = target - M_PI*2.0;
+        }
+    }
+    else if(turnFlag == TURN_RIGHT)
+    {
+        target = start_yaw - M_PI/2.0;
+        if(target < -M_PI)
+        {
+            target = target + M_PI*2.0;
+        }
+    }
+    else //turn back
+    {
+        target = start_yaw + M_PI;
+        if(target > M_PI)
+        {
+            target = target - M_PI*2.0;
+        }
+    }
+
+    //开始不断的计算偏差,调整角度
+    while(fabs(diff) > 0.01)
+    {
+        yawSrvClient.call(yawSrv);
+        now_yaw = yawSrv.response.yaw;
+        ROS_INFO("Now Yaw data:%f, Target Yaw:%f", now_yaw, target);
+
+        diff = target - now_yaw;
+        if(diff > 0)
+        {
+            angular = angular_speed;
+        }
+        else
+        {
+            angular = -angular_speed;
+        }
+        ROS_WARN("Yaw diff:%f, angular:%f", diff, angular);
+        publishTwistCmd(0, angular);
+        ros::Duration(0.04).sleep();
+    }
+    publishTwistCmd(0, 0); //在修正角度结束后应该立刻停止小车转动
+    ROS_INFO("Correct Yaw diff over !");
+}
+
+/****************************************************
+ * Description:控制小车前进移动0.5m
+ ***************************************************/
+void goForward(const float check_dist)
+{
+    tf::TransformListener tf_Listener;
+    tf::StampedTransform tf_Transform;
+    float dist = 0.0; //每次前进移动的距离
+    float front_dist = 0.0;
+
+    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();
+
+    distSrvClient.call(distSrv); //通过服务调用来获取测距值
+    front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
+    ROS_INFO("Front dist:%f", front_dist);
+    while((dist<check_dist)&&(front_dist>tolerance_dist*4.0)&&(ros::ok()))
+    {
+        ROS_INFO("Go forward, dist:%f", dist);
+        publishTwistCmd(linear_x_speed, 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));
+
+        distSrvClient.call(distSrv); //通过服务调用来获取测距值
+        front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
+        ROS_INFO("Front dist:%f", front_dist);
+    }
+    publishTwistCmd(0, 0); //stop move
+    ROS_INFO("Go forward finish !!!");
+}
+
+/*************************************************************
+ * Description:控制小车左右转90度,或者掉头180度.这里实现小车
+ *   转弯功能是通过不断监听odom和base_link之间的tf转换完成的.
+ ************************************************************/
+int controlTurn(int flag)
+{
+    tf::TransformListener tf_Listener;
+    tf::StampedTransform tf_Transform;
+    float angular = 0.0;
+    float check_angle = 0.0;
+
+    switch(flag)
+    {
+        case TURN_LEFT: //向左转动90°
+            turnFlag = TURN_LEFT;
+            angular = angular_speed;
+            check_angle = M_PI/2.0;
+            break;
+
+        case TURN_RIGHT: //向右转动90°
+            turnFlag = TURN_RIGHT;
+            angular = -angular_speed;
+            check_angle = M_PI/2.0;
+            break;
+
+        case TURN_BACK: //小车原地掉头180度
+            turnFlag = TURN_BACK;
+            check_angle = M_PI;
+            angular = angular_speed;
+            break;
+
+        case GO_FORWARD: //小车直行50cm
+            goForward(forward_dist);
+            return 1;
+
+        default:
+            ROS_ERROR("Turn flag error !");
+            return -1;
+    }
+
+    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();
+
+        return -2;
+    }
+
+    //ROS_INFO("Check_angle:%f, angular_speed:%f",check_angle, angular);
+    tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+    float last_angle = fabs(tf::getYaw(tf_Transform.getRotation()));
+    float turn_angle = 0.0;
+    ROS_INFO("first_angle:%f", last_angle);
+    while((fabs(turn_angle) < check_angle)&&(ros::ok()))
+    {
+        publishTwistCmd(0, angular); //原地转弯,不需要linear_x,y的控制
+        ros::Duration(0.10).sleep(); //100 ms
+
+        try
+        {
+            tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+        }
+        catch(tf::TransformException &ex)
+        {
+            ROS_ERROR("%s", ex.what());
+            continue;
+        }
+        float rotation = fabs(tf::getYaw(tf_Transform.getRotation()));
+        float delta_angle = fabs(rotation - last_angle);
+
+        turn_angle += delta_angle;
+        last_angle = rotation;
+        ROS_INFO("Turn angle:%f", turn_angle);
+    }
+    publishTwistCmd(0, 0); //原地转弯完成后,需要立刻停止小车的旋转
+    ROS_INFO("Turning finish !!!");
+    ros::Duration(0.1).sleep(); //100 ms
+
+    return 0;
+}
+
+/***************************************************************************
+ * Description:检查三个传感器数据,然后控制小车移动.这里需要防止小车在原地
+ *   进行连续两次旋转,在每次转弯后必须有个直行.这里需要注意的是当小车在转弯
+ *   后一定要有一个可以修正转弯角度的功能,否则转弯后角度不准确小车前进方向
+ *   会错.这里的走迷宫策略是左转优先,中间是直行,最后是右转.
+ *   当前方,左边,右边都无法移动时,进行掉头动作,然后直行.
+ **************************************************************************/
+void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
+        float tolerance, float warn_distance)
+{
+    static int flag = 0; //记录是否转过弯的标志
+
+    yawSrvClient.call(yawSrv); //通过服务调用来获取yaw值
+    start_yaw_data = yawSrv.response.yaw; //记录当前yaw值,用于后面修正
+
+    //判断能否左转,并且上一次动作不能是转弯
+    if((infrared_l == (float)0.3)&&(flag != 1))
+    {
+        ROS_WARN("turn Left! yaw_data:%f", start_yaw_data);
+        turnFlag = TURN_LEFT;
+        flag = 1; //设置转弯标志
+    }
+    else if(infrared_f > warn_distance)//判断能否直行
+    {
+        //始终控制小车保持在过道的中间位置
+        ROS_WARN("go Forward ! yaw_data: %f", start_yaw_data);
+        turnFlag = GO_FORWARD;
+        flag = 0; //设置直行标志
+    }
+    else if((infrared_r == (float)0.3)&&(flag != 1))//判断能否右转
+    {
+        ROS_WARN("turn Right ! yaw_data: %f", start_yaw_data);
+        turnFlag = TURN_RIGHT;
+        flag = 1; //设置转弯标志
+    }
+    else //小车掉头
+    {
+        ROS_WARN("turn Back ! yaw_data: %f", start_yaw_data);
+        turnFlag = TURN_BACK;
+    }
+
+    controlTurn(turnFlag); //开始执行动作
+}
+
+/*****************************************************************************
+ * Description: 订阅红外测距传感器话题后的回调函数,根据三个传感器数据进行走
+ *   迷宫.
+ ****************************************************************************/
+void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
+{
+    ROS_INFO("front left right distance:[%f %f %f]", msg->front_dist,
+            msg->left_dist, msg->right_dist);
+    front_dist = msg->front_dist;
+    left_dist  = msg->left_dist;
+    right_dist = msg->right_dist;
+
+    //根据三个传感器反馈的测距值开始移动
+    checkInfraredDist(front_dist, left_dist, right_dist, tolerance_dist, warn_dist);
+    yawUpdate(start_yaw_data); //移动完成后开始更新yaw值
+}
+
+/************************************************************
+ * Description:主函数初始化节点,读取配置参数,订阅和发布话题,
+ *   在订阅的红外测距数据话题回调函数中进行移动,通过调用yaw
+ *   服务进行转弯角度的修正.
+ ************************************************************/
+int main(int argc, char **argv)
+{
+    std::string cmd_topic;
+    std::string yaw_service;
+    std::string yaw_zero_service;
+
+    ros::ServiceClient yawZeroClient;
+
+    ros::init(argc, argv, "drive_square_node");
+    ros::NodeHandle handle;
+
+    ros::param::get("~cmd_topic", cmd_topic);
+    ros::param::get("~yaw_service", yaw_service);
+    ros::param::get("~yaw_zero_service", yaw_zero_service);
+    ros::param::get("~linear_x",  linear_x_speed);
+    ros::param::get("~tolerance_dist", tolerance_dist);
+    ros::param::get("~odom_frame", odomFrame);
+    ros::param::get("~base_frame", baseFrame);
+
+    twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
+    yawSrvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+
+    yawZeroClient = handle.serviceClient<rasp_imu_hat_6dof::SetYawZero>(yaw_zero_service);
+    rasp_imu_hat_6dof::SetYawZero yawZeroSrv;
+    yawZerlient.call(yawZeroSrv);
+    ROS_INFO("IMU yaw data zero");
+
+    ros::spin();
+    return 0;
+}
+