第十一课，ROS与传感器

1、Kinect

1）安装

sudo apt-get install ros-indigo-openni-camera

sudo apt-get install ros-indigo-openni-launch

2）驱动

https://github.com/avin2/SensorKinect

运行之后添加PointCloud2在该框架下的主题选择为/camera/depth_register ，以及把Fixed Frame改成camera_link

以后尽量不使用虚拟机，因为它对真实设备的支持不怎么好。

2、Ladar

使用国产的rqlidar，成本比较低，

驱动的下载https://github.com/robopeak/rplidar_ros，放在catkin的workspace下面的src目录下，然后对其进行编译catkin_make

运行：

roslaunch rplidar_ros rplidar.launch

注意事项：

在运行之前需要修改port的权限

sudo chmod 777 ttyUSB0(或者AMC0或者AMC1)

运行roslaunch rplidar_ros view_rplidar.launch

3、Arduino

它是一个开源的硬件，是ROS支持最好的一款单片机，

运行之后会产生一系列的库文件，

4、发布传感器信息（fake sensor）

首先我们讲解的是fake odometry以及fake laser。

posewithcovariance 的消息定义是一个point类型（x,y,z）以及一个四元组的类型(x,y,z,w)，以及一个6*6的协方差矩阵

TwistWithCovariance的消息定义是一个角速度和线速度，以及一个6*6的协方差矩阵

1、创建一个软件包

catkin_create_pkg fake_sensor roscpp  tf

2、编译之catkin_make

3、创建一个源文件odometry.cpp

#include<ros/ros.h>

#include<tf/transform_boradcaster.h>//tf转换的头文件

#include<nav_msgs/Odometry.h>//导航信息的头文件

int main(int argc,char **argv)

{

　　ros::init(argc,argv,"state_publisher");//节点名字为state_publisher

　　ros::NodeHandle n;

　　//创建一个publisher，用来发布消息类型为nav_msgs::Odometry，发布到主题为odom上

　　ros::Publisher odom_pub=n.advertise<nav_msgs::Odometry>("odom",10);

　　//创建三个变量来初始化位置

　　double x=0.0;

　　double y=0.0;

　　double th=0.0;

　　//初始化速度

 　　double vx=0.4;

　　double vy=0;

　　double vth=0.2;//角速度

　　//创建两个时间对象用来记录发生的时间，用时间相减就的到δt

　　ros::Time current_time=ros::Time::now();

　　ros::Time last_time=ros::Time::now();

　　//创建一个tf树中广播器用来广播tf变换

　　tf::TransformBroadcaster broadcaster;

　　ros::Rate rate(10);//消息发布的频率

　　//储存广播的tf变换

　　geometry_msgs::TransformStamped odom_trans;

　　//对其成员变量赋值

　　odom_trans.header.frame_id="odom";//父坐标系为odom

　　//child的frame_id设为如下,那么子坐标系为base_footprint

　　odom_trans.child.frame_id="base_footprint";

　　//下面来完成位置信息的变换

　　while(ros::ok())

　　{

　　//更新一下current_time

　　current_time=ros::Time::now();

　　//计算位置变换和姿态的变换

　　double dt=(current_time-last_time).toSec();//当前时间减去过去时间，并转换成秒

　　double delta_x=(vx*cos(th)-vy*sin(th))*dt;

　　double delta_y=(vx*sin(th)+vy*cos(th))*dt;

　　double delta_th=vth*dt;

　　//下面更新x,y,以及theta的值

　　x+=delta_x;

　　y+=delta_y;

　　th+=delta_th;

　　//创建一个四元组,下面返回的是一个geometry_msgs::Quaternion类型的消息

　　geometry_msgs::Quaternion odom_quat=tf::createQuaternionMsgFromYaw(th);

　　//下面更新一下transform消息

　　odom_trans.header.stamp=current_time;

　　odom_trans.transform.translation.x=x;

　　odom_trans.transform.translation.y=y;

　　odom_trans.transform.translation.z=0;

　　odom_trans.transform.rotation=odo_quat;

　　nav_msgs::Odometry odom;

　　odom.header.stamp=current_time;

　　odom.header.frame_id="odom";

　　odom.child.frame_id="base_footprint";

　　odom.pose.pose.position.x=x;

　　odom.pose.pose.position.y=y;

　　odom.pose.pose.position.z=0;

　　odom.twist.twist.linear.x=vx;

　　odom.twist.twist.linear.y=vy;

　　odom.twist.twist.linear.z=0;

　　odom.twist.twist.angular.z=vth;

　　odom.twist.twist.angular.x=0;

　　odom.twist.twist.angular.y=0;

　　last_time=current_time;

　　//把odometry_transform消息发布出去，以及把odom消息发布在odometry上

　　broadcaster.sendTransform(odom_trans);

　　odom_pub.publish(odom);

　　}

　　rate.sleep();

　　return 0;

}

回顾一下在odometry里面做了哪些内容

首先创建了一个消息类型为nav_msgs::Odometry的发布者，发布主题为odom；

然后又创建了一个tf::TransformBroadcaster广播tf_transform变换.

接着创建了两个对应的消息，一个是geometry_msgs::TransformStamped用来储存tf变换的内容以及一个nav_msgs::Odometry的odom消息，用于更新它的位置信息以及姿态信息；然后通过odom_pub.publish(odom)发布出去。

以及利用broadcaster.sendTransform()方法发送odom_trans的tf变换。

下面修改CMakeLists.txt文件

add_executable(odometry src/odometry.cpp)

target_link_libraries(odometry ${catkin_LIBRARIES})

编译之catkin_make

运行之

rosrun fake_sensor odometry

rosrun rviz rviz

添加一个主题odometry

固定坐标系Fixed frame选择odom

由于没有为orientation赋值，改正之后

再添加一个tf看一下的他带来的相对变换

固定坐标系是odom，目标坐标系是base_footprint。

下面再来看一下激光雷达该怎么写！！！！！！！！！！

1、创建一个源文件

laser.cpp

#include<ros/ros.h>

#include<sensor_msgs/LaserScan.h>//导航信息的头文件

int main(int argc,char **argv)

{

　　ros::init(argc,argv,"laser_scan_publisher");//节点名字为laser_scan_publisher

　　ros::NodeHandle n;

　　ros::Publisher scan_pub=n.advertise<sensor_msgs::LaserScan>("scan",50);

　　//创建一个扫面的步子

　　unsigned int num_readings=100;

　　//创建一个频率变量

　　double laser_frequency=40;

　　//储存没扫描一步得到的距离信息

　　double ranges[num_readings];

　　//创建一个强度数组

　　double intensities[num_readings];

　　//创建一个count用来计数为ranges，intensities赋值

　　int count;

　　ros::Rate rate(1);

　　//为ranges以及intensities赋值

　　while(n.ok())

　　{

　　//利用for循环来赋值

　　for(unsigned int i=0;i<num_readings;++i)

　　{

　　　　ranges[i]=count;

　　　　intensities[i]=count+100;

　　}

　　ros::Time scan_time=ros::Time::now();

　　//再创建一个laserscan的消息

　　sensor_msgs::LaserScan scan;

　　scan.header.stamp=scan_time;

　　scan.header.frame_id="base_link"

　　scan.angle.min=-1.57;//假设扫面范围是3.14弧度

　　scan.angle.max=+1.57;

　　scan.angle.increment=3.14/num_readings;

　　scan.time_increment=(1/laser_frequency)/num_readings;//扫描一次的时间，频率分之一再除以step的总数

　　scan.range_max=100;//扫描最大距离

　　scan.range_min=0;//扫描最小距离

　//　scan.ranges.resize(num_readings);

　//　scan.intensities::sensor_msgs::LaserScan();

　　//再由for循环给laserscan的ranges以及intensities赋值

　　for(unsigned int i=0;i<num_readings;++i)

　　{

　　　　scan.ranges[i]=ranges[i];

　　　　scan.intensities[i]=intensities[i];

　　}

　　　//上面消息处理完了，下面发布消息

　　　scan_pub.publish(scan);

　　 ++count;

　　rate.sleep();

}

}

看一下以上完成的工作：

首先创建了一个ros::publisher的发布者scan_pub，发布在scan主题上的sensor_msgs::LaserScan消息，然后创建两个数组并赋值，创建一个消息，把消息内的数组与上面的数组赋值，最后再发布消息。

进入到CMakeLists.txt文件

add_executable(laser src/laser.cpp)

target_link_libraries(laser ${catkin_LIBRARIES})

编译之catkin_make

运行rosrun fake_sensor laser

运行rostopic echo /scan -n 1