ros1仿真导航机器人基础传感器数据读取

仅为学习记录和一些自己的思考，不具有参考意义。

1 仿真环境

gazebo、rviz、ros1

2 机器人模型

<?xml version="1.0"?>
<robot name="wpb_home_gazebo"><link name="base_footprint"><visual><origin xyz="0 0 0" rpy="0 0 0" /><geometry><box size="0.05 0.05 0.001" /></geometry></visual>
</link><joint name="base_joint" type="fixed"><origin xyz="0 0 0" rpy="0 0 0" /><parent link="base_footprint"/><child link="base_link" />
</joint><!-- base -->
<link name="base_link"><visual><geometry><box size="0.01 0.01 0.001" /></geometry><origin rpy = "0 0 0" xyz = "0 0 0"/></visual>
</link><!-- body -->
<link name = "body_link"><visual><geometry><mesh filename="package://why_simulation/meshes/wpb_home/wpb_home_std.dae" scale="1 1 1"/></geometry><origin rpy = "1.57 0 1.57" xyz = "-.225 -0.225 0"/></visual><collision><origin xyz="0.001 0 .065" rpy="0 0 0" /><geometry><cylinder length="0.13" radius="0.226"/></geometry></collision><inertial><mass value="20"/><inertia ixx="4.00538" ixy="0.0" ixz="0.0" iyy="4.00538" iyz="0.0" izz="0.51076"/></inertial>
</link>
<joint name = "base_to_body" type = "fixed"><parent link = "base_link"/><child link = "body_link"/><origin rpy="0 0 0" xyz="0 0 0"/> <!--pos-->
</joint>
<!-- top of base -->
<link name = "base_top_link"><collision><origin xyz="0 0 0" rpy="0 0 0" /><geometry><box size="0.33 0.31 0.01"/></geometry></collision><inertial><mass value="0.01"/><inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/></inertial>
</link>
<joint name = "body_to_top" type = "fixed"><parent link = "body_link"/><child link = "base_top_link"/><origin rpy="0 0 0" xyz="0.01 0 0.2"/> <!--pos-->
</joint>
<!-- back -->
<link name = "body_back_link"><collision><origin xyz="0 0 0" rpy="0 0 0" /><geometry><box size="0.03 0.23 1.05"/></geometry></collision><inertial><mass value="0.01"/><inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/></inertial>
</link>
<joint name = "body_to_back" type = "fixed"><parent link = "base_top_link"/><child link = "body_back_link"/><origin rpy="0 0.31 0" xyz="-0.038 0 0.5"/> <!--pos-->
</joint>
<!-- head -->
<link name = "head_link"><collision><origin xyz="0 0 0" rpy="0 0 0" /><geometry><box size="0.07 0.28  0.06"/></geometry></collision><inertial><mass value="0.01"/><inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/></inertial>
</link>
<joint name = "body_to_head" type = "fixed"><parent link = "base_top_link"/><child link = "head_link"/><origin rpy="0 0.27 0" xyz="0.155 0 1.17"/> <!--pos-->
</joint>
<!-- front -->
<link name = "front_link"><collision><origin xyz="0 0 0" rpy="0 0 0" /><geometry><cylinder length="1.1" radius="0.01"/></geometry></collision><inertial><mass value="0.01"/><inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/></inertial>
</link>
<joint name = "body_to_front" type = "fixed"><parent link = "base_top_link"/><child link = "front_link"/><origin rpy="0 0 0" xyz="0.15 0 0.55"/> <!--pos-->
</joint><!-- Lidar -->
<link name = "laser"><visual><geometry><cylinder length="0.001" radius="0.001"/></geometry><origin rpy = "0 0 0" xyz = "0 0 0"/></visual>
</link>
<joint name="laser_joint" type="fixed"><origin rpy="0 0 0" xyz="0 0 0.15"  /> <!--pos--><parent link="base_link" /><child link="laser" />
</joint><!-- Kinect -->
<link name = "kinect2_dock"><visual><geometry><!-- <box size=".01 .25 .07"/>--><box size="0.001 0.001 0.001"/></geometry><origin rpy = "0 0 0" xyz = "0 0 0"/></visual>
</link>
<joint name="kinect_height" type="fixed"><parent link="base_link"/><child link="kinect2_dock"/><origin xyz="0.145 -0.013 1.37" rpy="0 0 0"/> 
</joint><link name = "kinect2_head_frame"><visual><geometry><box size="0.001 0.001 0.001"/></geometry><origin xyz = "0 0 0" rpy = "0 0 0"/></visual>
</link>
<!--kinect_pitch -->
<joint name="kinect_pitch" type="fixed"><origin xyz="0 0 0" rpy="0 0.5 0" /> <parent link="kinect2_dock" /><child link="kinect2_head_frame" />
</joint><link name = "kinect2_front_frame"><visual><geometry><box size="0.001 0.001 0.001"/></geometry><origin xyz = "0 0 0" rpy = "0 0 0"/></visual>
</link>
<joint name="kinect_head" type="fixed"><origin xyz="0 0 0" rpy=" 0 1.57 0" /> <parent link="kinect2_head_frame" /><child link="kinect2_front_frame" />
</joint>
<link name = "kinect2_ir_optical_frame"><visual><geometry><!-- <box size=".25 .04 .07"/>--><box size="0.001 0.001 0.001"/></geometry><origin xyz = "0 0 0" rpy = "0 0 0"/></visual>
</link>
<joint name="kinect_ir_trans" type="fixed"><origin xyz="0 0 0" rpy="0 0 -1.57" /> <parent link="kinect2_front_frame" /><child link="kinect2_ir_optical_frame" />
</joint><link name = "kinect2_camera_frame"><visual><geometry><box size="0.001 0.001 0.001"/></geometry><origin rpy = "0 0 0" xyz = "0 0 0" /></visual>
</link>
<joint name="kinect_camra_joint" type="fixed"><origin xyz="0 0 0" rpy="3.1415926 0 -1.5707963" /><parent link="kinect2_ir_optical_frame" /><child link="kinect2_camera_frame" />
</joint><link name = "kinect2_rgb_optical_frame"><visual><geometry><box size="0.001 0.001 0.001"/></geometry><origin rpy = "0 0 0" xyz = "0 0 0" /></visual>
</link>
<joint name="kinect_hd_joint" type="fixed"><origin xyz="0 0 0" rpy="0 1.5707963 0" /><parent link="kinect2_camera_frame" /><child link="kinect2_rgb_optical_frame" />
</joint><!-- Gazebo plugin for WPR -->
<gazebo><plugin name="base_controller" filename="libwpr_plugin.so"><publishOdometryTf>true</publishOdometryTf><commandTopic>cmd_vel</commandTopic><odometryTopic>odom</odometryTopic><odometryFrame>odom</odometryFrame><odometryRate>20.0</odometryRate><robotBaseFrame>base_footprint</robotBaseFrame></plugin>
</gazebo><!-- Gazebo plugin for RpLidar A2 -->
<gazebo reference="laser"><sensor type="ray" name="rplidar_sensor"><pose>0 0 0.06 0 0 0</pose><visualize>true</visualize><update_rate>10</update_rate><ray><scan><horizontal><samples>360</samples><resolution>1</resolution><min_angle>-3.14159265</min_angle><max_angle>3.14159265</max_angle></horizontal></scan><range><min>0.24</min><max>6.0</max><resolution>0.01</resolution></range><noise><type>gaussian</type><mean>0.0</mean><stddev>0.01</stddev></noise></ray><plugin name="rplidar_ros_controller" filename="libgazebo_ros_laser.so"><topicName>scan</topicName><frameName>laser</frameName></plugin></sensor>
</gazebo><!-- Gazebo plugin for Kinect v2 -->
<gazebo reference="kinect2_head_frame"><sensor type="depth" name="kinect2_depth_sensor" ><always_on>true</always_on><update_rate>10.0</update_rate><camera name="kinect2_depth_sensor"><horizontal_fov>1.221730456</horizontal_fov><image><width>512</width><height>424</height><format>B8G8R8</format></image><clip><near>0.5</near><far>6.0</far></clip><noise><type>gaussian</type><mean>0.1</mean><stddev>0.07</stddev></noise></camera><plugin name="kinect2_depth_control" filename="libgazebo_ros_openni_kinect.so"><cameraName>kinect2/sd</cameraName><alwaysOn>true</alwaysOn><updateRate>20.0</updateRate><imageTopicName>image_ir_rect</imageTopicName><depthImageTopicName>image_depth_rect</depthImageTopicName><pointCloudTopicName>points</pointCloudTopicName><cameraInfoTopicName>depth_camera_info</cameraInfoTopicName><frameName>kinect2_ir_optical_frame</frameName><pointCloudCutoff>0.5</pointCloudCutoff><pointCloudCutoffMax>6.0</pointCloudCutoffMax><baseline>0.1</baseline><distortionK1>0.0</distortionK1><distortionK2>0.0</distortionK2><distortionK3>0.0</distortionK3><distortionT1>0.0</distortionT1><distortionT2>0.0</distortionT2></plugin></sensor>
</gazebo>
<gazebo reference="kinect2_rgb_optical_frame"><sensor type="camera" name="kinect2_rgb_sensor"><always_on>true</always_on><update_rate>20.0</update_rate><camera name="kinect2_rgb_sensor"><horizontal_fov>1.221730456</horizontal_fov><image><width>1920</width><height>1080</height><format>B8G8R8</format></image><clip><near>0.2</near><far>10.0</far></clip><noise><type>gaussian</type><mean>0.0</mean><stddev>0.007</stddev></noise></camera><plugin name="kinect2_rgb_controller" filename="libgazebo_ros_camera.so"><alwaysOn>true</alwaysOn><update_rate>20.0</update_rate><cameraName>kinect2/hd</cameraName><imageTopicName>image_color_rect</imageTopicName><cameraInfoTopicName>camera_info</cameraInfoTopicName><frameName>kinect2_rgb_optical_frame</frameName></plugin></sensor>
</gazebo><gazebo reference="kinect2_head_frame"><sensor type="camera" name="kinect2_qhd_rgb_sensor"><always_on>true</always_on><update_rate>20.0</update_rate><camera name="kinect2_qhd_rgb_sensor"><horizontal_fov>1.221730456</horizontal_fov><image><width>960</width><height>540</height><format>R8G8B8</format></image><clip><near>0.2</near><far>10.0</far></clip></camera><plugin name="kinect2_qhd_rgb_controller" filename="libgazebo_ros_camera.so"><alwaysOn>true</alwaysOn><update_rate>20.0</update_rate><cameraName>kinect2/qhd</cameraName><imageTopicName>image_color_rect</imageTopicName><cameraInfoTopicName>camera_info</cameraInfoTopicName><frameName>kinect2_head_frame</frameName></plugin></sensor>
</gazebo><!-- IMU plugin for 'body_link' -->
<gazebo reference="body_link"><gravity>true</gravity><sensor name="imu_sensor" type="imu"><always_on>true</always_on><update_rate>100</update_rate><visualize>true</visualize><topic>__default_topic__</topic><plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin"><topicName>imu/data</topicName><bodyName>body_link</bodyName><updateRateHZ>100.0</updateRateHZ><gaussianNoise>0.0</gaussianNoise><xyzOffset>0 0 0</xyzOffset><rpyOffset>0 0 0</rpyOffset><frameName>imu_link</frameName></plugin><pose>0 0 0 0 0 0</pose></sensor>
</gazebo></robot>

3启动launch文件

roslaunch why_simulation why_simple.launch

<launch><!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(find why_simulation)/worlds/why_simple.world"/><arg name="paused" value="false"/><arg name="use_sim_time" value="true"/><arg name="gui" value="true"/><arg name="recording" value="false"/><arg name="debug" value="false"/></include><!-- Spawn the objects into Gazebo --><node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find why_simulation)/models/bookshelft.model -x 3.0 -y 0.2 -z 0 -Y 3.14159 -urdf -model bookshelft" /><node name="bottle" pkg="gazebo_ros" type="spawn_model" args="-file $(find why_simulation)/models/bottles/red_bottle.model -x 2.8 -y 0 -z 0.6 -Y 0 -urdf -model red_bottle" /><!-- Spawn a robot into Gazebo --><node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find why_simulation)/models/wpb_home.model -urdf -model wpb_home" /><!-- Robot Description --><arg name="model" default="$(find why_simulation)/models/wpb_home.model"/><param name="robot_description" command="$(find xacro)/xacro $(arg model)" /><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>

rviz

4Topic话题查看

rostopic list

/clock
/cmd_vel
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/imu/data
/joint_states
/kinect2/hd/camera_info
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_color_rect/compressed/parameter_descriptions
/kinect2/hd/image_color_rect/compressed/parameter_updates
/kinect2/hd/image_color_rect/compressedDepth
/kinect2/hd/image_color_rect/compressedDepth/parameter_descriptions
/kinect2/hd/image_color_rect/compressedDepth/parameter_updates
/kinect2/hd/image_color_rect/theora
/kinect2/hd/image_color_rect/theora/parameter_descriptions
/kinect2/hd/image_color_rect/theora/parameter_updates
/kinect2/hd/parameter_descriptions
/kinect2/hd/parameter_updates
/kinect2/qhd/camera_info
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_color_rect/compressed/parameter_descriptions
/kinect2/qhd/image_color_rect/compressed/parameter_updates
/kinect2/qhd/image_color_rect/compressedDepth
/kinect2/qhd/image_color_rect/compressedDepth/parameter_descriptions
/kinect2/qhd/image_color_rect/compressedDepth/parameter_updates
/kinect2/qhd/image_color_rect/theora
/kinect2/qhd/image_color_rect/theora/parameter_descriptions
/kinect2/qhd/image_color_rect/theora/parameter_updates
/kinect2/qhd/parameter_descriptions
/kinect2/qhd/parameter_updates
/kinect2/sd/depth/camera_info
/kinect2/sd/depth_camera_info
/kinect2/sd/image_depth_rect
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/image_ir_rect/compressed/parameter_descriptions
/kinect2/sd/image_ir_rect/compressed/parameter_updates
/kinect2/sd/image_ir_rect/compressedDepth
/kinect2/sd/image_ir_rect/compressedDepth/parameter_descriptions
/kinect2/sd/image_ir_rect/compressedDepth/parameter_updates
/kinect2/sd/image_ir_rect/theora
/kinect2/sd/image_ir_rect/theora/parameter_descriptions
/kinect2/sd/image_ir_rect/theora/parameter_updates
/kinect2/sd/parameter_descriptions
/kinect2/sd/parameter_updates
/kinect2/sd/points
/odom
/rosout
/rosout_agg
/scan
/tf
/tf_static

5运动控制与传感器数据读取

运动控制

rosrun why_test test_vel

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>int main(int argc, char  *argv[])
{ros::init(argc,argv,"test_vel");ros::NodeHandle nh;ros::Publisher pub_=nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);ros::Rate rate(10);while (ros::ok()){auto twist=geometry_msgs::Twist();twist.linear.x=0.2;twist.angular.z=0.2;pub_.publish(twist);rate.sleep();}return 0;
}

激光雷达数据读取

rosrun why_test test_lidar

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>void LidarCallback(const sensor_msgs::LaserScan msg)
{float dMidDist=msg.ranges[180];std::cout<<"distance="<<dMidDist<<std::endl;
}int main(int argc, char *argv[])
{ros::init(argc,argv,"test_lidar");ros::NodeHandle nh;ros::Subscriber sub_=nh.subscribe("/scan",10,&LidarCallback);ros::spin();return 0;
}

IMU数据读取

rosrun why_test test_imu

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"void IMUCallback(const sensor_msgs::Imu msg)
{if(msg.orientation_covariance[0] < 0)return;tf::Quaternion quaternion(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w);double roll, pitch, yaw;tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);roll = roll*180/M_PI;pitch = pitch*180/M_PI;yaw = yaw*180/M_PI;ROS_INFO("roll= %.0f pitch= %.0f yaw= %.0f", roll, pitch, yaw);
}int main(int argc, char **argv)
{ros::init(argc,argv, "test_imu"); ros::NodeHandle n;ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);ros::spin();return 0;
}