|
|
@@ -1,369 +0,0 @@
|
|
|
-/***************************************************************
|
|
|
- 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;
|
|
|
-}
|
|
|
-
|
