|
大家好，我是一名“黑科技”发烧友，最近听闻北醒新推出一款固态激光雷达CE30-A，赶紧入手了一台尝尝鲜，基于移动小车底盘和北醒提供的SDK，简单做了几个小功能，先来看看效果： 可以看到视频里雷达的整体效果还是不错的，现在和各位大神和新手朋友分享一下我的开发过程吧，首先是一些基础的准备： 1、 系统要求： Ubuntu14.04，安装ROS Indigo系统。 2、 ROS系统在ubuntu下安装： 3、编译CE30 ROS官方驱动包： (2). 将官方驱动ce30_ros文件夹解压后复制到工作空间下的src文件夹下。 4、编译ce30应用程序。 (1). 将ce30_use文件夹解压后复制到创建的工作空间的src文件夹下。 (2). 参照上述的流程编译ce30_use。 5、启动应用程序 (1)、将兼容ROS的机器人底盘连接到工控机。 (2)、按Ctrl+alt+T键打开terminal。 (3)、给串口设置权限，在terminal中输入sudo chmod 777 /dev/ttyUSB*。 (4)、如果要使用避障和绕障模式，输入roslaunch ce30_use use.launch。 (5)、避障模式和绕障模式通过use.launch文件中的is_avoid参数切换，如果想使用绕障模式，将is_avoid的value值设置为true，否则设置为false。 为了更直观的表达我的思路，下面画了2个流程图： 1、 指定车宽避障模式程序流程图（见图1） 图1 避障模式程序流程图 2、绕障模式程序流程图（见图2） 图2 机器人绕障模式 3、跟车模式算法流程 （1） 定义PID结构体（其中比例系数P，积分系数I，微分系数D，目标量target,当前误差为iEorro，累计误差为InEorro，微分误差为dEorro，最后误差为lastEorro）。 （2） 初始化程序。 （3） 接收雷达数据为laserData。 （4） 计算当前误差iEorro = target – laserData； a) 累计误差InEorro = InEorro + iEorro； b) 微分误差dEorro = iEorro – lastEorro。 （5） 赋值lastEorro = iEorro。 （6） 计算控制量V = P*iEorro + I*InEorro +D*dEorro。 （7） 将控制量发送给底盘执行。 （8） 重复步骤（3）至步骤（7） 以上是我的基本思路和想法，下面贴上代码 Obstacle Avoidance.cpp Vehicle Follow.cpp 各位大神见笑了，还请高手指点，新手朋友有问题的话欢迎交流讨论！
#include <iostream> #include <ce30_msgs/Ce30Data.h> #include "ros/ros.h" #include <ce30_use/control.h> #include <geometry_msgs/Twist.h>
ros::Publisher pubStop; ros::Publisher pubForward; bool is_avoid;
void Stop(); //stop the robot void Forward(); // let robot go forward void RobotMove(); // go forward void Rotate(float angular); // make robot rotate in place bool Check(float depth); //check if there is obstacle in front of the robot void avoid(float depth, float angular); //to avoid the obstacle using in avoid mode.
void lidarCallback(const ce30_msgs::Ce30Data::ConstPtr &msg) //get lidar data { float depth, angular; bool IsCollision; //ROS_ERROR("MIAO"); if(!is_avoid) //check if it is avoid mode { depth = msg->depth; IsCollision = Check(depth); if(IsCollision) { ROS_ERROR("Stop!!"); Stop(); } else { RobotMove(); } } else { depth = msg->depth; angular = msg->angle; avoid(depth, angular); } }
int main(int argc, char **argv) { ros::init(argc, argv, "ce30_use"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("ce30_data",1000, lidarCallback); //initialize the ROS subscribe proces
n.param("avoid_mode", is_avoid, false); pubStop = n.advertise<geometry_msgs::Twist>("cmd_vel_mux/input/safety_controller",1000); pubForward = n.advertise<geometry_msgs::Twist>("cmd_vel_mux/input/navi",1000); ros::spin(); return 0; }
void Stop() { geometry_msgs::Twist msg; msg.angular.x = 0.0; msg.angular.y = 0.0; msg.angular.z = 0.0; msg.linear.x = 0.0; msg.linear.z = 0.0; msg.linear.y = 0.0; pubStop.publish(msg); }
void Forward() { geometry_msgs::Twist msg; msg.angular.x = 0.0; msg.angular.y = 0.0; msg.angular.z = 0.0; msg.linear.x = 0.2; msg.linear.y = 0.0; msg.linear.z = 0.0; pubForward.publish(msg); }
void RobotMove() { Forward(); }
void Rotate(float angular) { geometry_msgs::Twist msg; msg.angular.x = 0.0; msg.angular.y = 0.0; msg.angular.z = angular; msg.linear.x = 0.0; msg.linear.y = 0.0; msg.linear.z = 0.0; pubStop.publish(msg); }
bool Check(float depth) { if(depth > 0 && depth < 0.25) { return true; } else { return false; } }
void avoid(float depth, float angular) { if(depth > 0 && depth < 0.3) { if(angular >= 0) { ROS_ERROR("Rotate Left!"); Rotate(0.6); } else { ROS_ERROR("Rotate Right!"); Rotate(-0.6); } }
else { return; } } 第二部分 #include <iostream> #include <ce30_msgs/Ce30Data.h> #include <ros/ros.h> #include <geometry_msgs/Twist.h> #include <nav_msgs/Odometry.h>
#define DISTANCE 0.35 #define P_DATA 1.5 #define I_DATA 0.08 #define D_DATA 0.1
typedef struct PID{ float SetPoint; //设定目标Desired Value float Proportion; //比例常数Proportional Const float Integral; //积分常数Integral Const float Derivative; //微分常数Derivative Const float LastError; //Error[-1] float InError; //Error[-2] }PID;
ros::Publisher pub; geometry_msgs::Twist robot_control; nav_msgs::Odometry odom_old; PID pid; ce30_msgs::Ce30Data ce30_old;
void init(); void PIDInit(); void lidarcallback(const ce30_msgs::Ce30Data &recv_data); void odomcallback(const nav_msgs::Odometry &recv_odom); float PIDControl(const ce30_msgs::Ce30Data &lidar); void pub_v();
void init() { robot_control.angular.z = 0.0; robot_control.linear.x = 0.0; odom_old.twist.twist.linear.x = 0.0; odom_old.twist.twist.angular.z = 0.0; PIDInit(); }
void lidarcallback(const ce30_msgs::Ce30Data &recv_data) { float lin, ang; if(recv_data.depth != 0) { lin = -1.0*PIDControl(recv_data); if(fabs(recv_data.angle)>0.6){ ang = -0.5*recv_data.angle; }else{ ang = 0; } robot_control.linear.x = lin; robot_control.angular.z = ang; } ce30_old = recv_data; pub_v(); }
void odomcallback(const nav_msgs::Odometry &recv_odom) { odom_old = recv_odom; }
void PIDInit() { pid.LastError = 0; //Error[-1] pid.InError = 0; //Error[-2] pid.Proportion = P_DATA; //比例常数Proportional Const pid.Integral = I_DATA; //积分常数Integral Const pid.Derivative = D_DATA; //微分常数Derivative Const pid.SetPoint = DISTANCE; //目标是100 } float PIDControl(const ce30_msgs::Ce30Data &lidar) { float iError, iIncpid, NextPoint; NextPoint = lidar.depth; iError = pid.SetPoint - NextPoint; //当前误差 pid.InError += iError; //存储误差，用于下次计算 iIncpid = pid.Proportion * iError //E[k]项 + pid.Integral * pid.InError //E[k－1]项 + pid.Derivative * (iError - pid.LastError); //E[k－2]项 pid.LastError = iError; return iIncpid; } void pub_v() { pub.publish(robot_control); }
int main(int argc, char **argv) { ROS_INFO("ROBOT FOLLOW"); ros::init(argc, argv, "follow"); init(); ros::NodeHandle n; ros::Subscriber sub_lidar = n.subscribe("ce30_data",1, lidarcallback); ros::Subscriber sub_odom = n.subscribe("odom",1, odomcallback); pub = n.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity",1000); ros::spin(); }
| 
/2 
发表于 2017-11-27 10:34
收藏
