第十一课，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
