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;
}