ROS学习记录13【SLAM】仿真学习2——创建一个包含Lidar、Camera、IMU的Ackman小车

于 2022-01-09 17:32:31 发布
阅读量1.4k 收藏 19
点赞数 5
分类专栏： ROS学习记录 文章标签： 自动驾驶 ros ackman 仿真 gazebo
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/u011017694/article/details/122390353
版权

零.原理

原理就是一个普通的小车，配合上Gazebo plugins实现Gazebo里的信息采集。
对了我目前打算的是个后驱车，如果要四驱的，可以自己给前轮的Joint加两个velocity_controllers即可。

一.造车

1.1 写一个车

先在src/slam_model/urdf/ackman.urdf写一个车：

<robot name="ackman">  
    <!--Main Body Begin-->
    <link name="base_footprint">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
            <box size="0.001 0.001 0.001"/>
            </geometry>
        </visual>
    </link>

    <joint name="base_footprint_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
        <parent link="base_footprint"/>
        <child link="base_link"/>
    </joint>

    <link name="base_link">
        <inertial>
            <origin xyz="0 0 0"/>
            <mass value="300"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>
        <visual>
            <geometry>
                <box size="0.8 0.4 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="blue">
                <color rgba="0.5 0.2 0.8 1"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.8 0.4 0.1"/>
            </geometry>
            <inertial>
                <mass value="300"/>
                <inertia ixx="0.0053" ixy="0" ixz="0" iyy="0.0083" iyz="0" izz="0.0667"/>
            </inertial>
        </collision>
    </link>

    <link name="base_wheel">
        <inertial>
            <origin xyz="0 0 0"/>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
        <visual>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="blue">
                <color rgba="0.5 0.2 0.8 1"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <inertial>
                <mass value="0.1"/>
                <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
            </inertial>
        </collision>
    </link>

    <joint name="base_wheel_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
        <parent link="base_link"/>
        <child link="base_wheel"/>
    </joint>

	<joint name="left_rear_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="base_wheel"/>
		<child link="left_rear_wheel"/>
		<origin rpy="1.57 0 0" xyz="-0.3 0.3 0"/>
		<limit effort="10" velocity="100"/>
		<joint_properties damping="0.0" friction="0.0"/>    
	</joint>

    <link name="left_rear_wheel">
        <inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
            </inertial>
		</collision>
	</link>

    <joint name="right_rear_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="base_wheel"/>
		<child link="right_rear_wheel"/>
		<origin rpy="1.57 0 0" xyz="-0.3 -0.3 0"/>
		<limit effort="10" velocity="100"/>
		<joint_properties damping="0.0" friction="0.0"/>    
	</joint>

    <link name="right_rear_wheel">
        <inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
            </inertial>
		</collision>
	</link>
    
    <joint name="left_bridge_joint" type="revolute">
        <parent link="base_wheel"/>
        <child link="left_bridge"/>
        <origin rpy="0 0 0" xyz="0.3 0.255 0"/>  <!--Y=0.25-0.5*whell_length-0.5*brige_y-->
        <axis xyz="0 0 1"/>
        <limit effort="2.5" lower="-0.758" upper="0.758" velocity="1"/>
    </joint>

    <link name="left_bridge">
        <inertial>
            <origin xyz="0.0 0 0"/>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
        <visual>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <material name="red">
            <color rgba="1 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <inertial>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
        </collision>
    </link>

    <joint name="left_front_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="left_bridge"/>
		<child link="left_front_wheel"/>
		<origin rpy="1.57 0 0" xyz="0 0.045 0"/> <!--Z=0.5*whell_length+0.5*brige_y-->
	</joint>

    <link name="left_front_wheel">
		<inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
		</collision>
	</link>

    <!--right bridge and wheel are mirrored from left-->
    <joint name="right_bridge_joint" type="revolute">
        <parent link="base_wheel"/>
        <child link="right_bridge"/>
        <origin rpy="0 0 0" xyz="0.3 -0.255 0"/>  <!--Y=0.25-0.5*whell_length-0.5*brige_y-->
        <axis xyz="0 0 1"/>
        <limit effort="2.5" lower="-0.758" upper="0.758" velocity="1"/>
    </joint>

    <link name="right_bridge">
        <inertial>
            <origin xyz="0.0 0 0"/>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
        <visual>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <material name="red">
            <color rgba="1 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <inertial>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
        </collision>
    </link>

    <joint name="right_front_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="right_bridge"/>
		<child link="right_front_wheel"/>
		<origin rpy="1.57 0 0" xyz="0 -0.045 0"/> <!--Z=0.5*whell_length+0.5*brige_y-->
	</joint>

    <link name="right_front_wheel">
		<inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            <cylinder radius="0.1" length="0.05"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
		</collision>
	</link>

    <gazebo reference="base_link">
        <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="left_rear_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="right_rear_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="left_front_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="right_front_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="left_bridge">
        <material>Gazebo/Red</material>
    </gazebo>
    <gazebo reference="right_bridge">
        <material>Gazebo/Red</material>
    </gazebo>
    <!--Main Body End-->

    <!--car's movement controllers begin-->
    <transmission name="right_wheel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="right_rear_wheel_joint">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="right_wheel_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <transmission name="left_wheel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="left_rear_wheel_joint">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="left_wheel_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    
    <!-- dont add these controller into urdf, or you will find a funny movement -->
    <!-- car's front wheel will keep 0m/s because of the controller
    <transmission name="right_front_wheel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="right_front_wheel_joint">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="right_front_wheel_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    -->
    <transmission name="right_bridge">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="right_bridge_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="right_bridge_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <transmission name="left_bridge">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="left_bridge_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="left_bridge_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="ros_control">
            <robotNamespace>/ackman</robotNamespace>
            <legacyModeNS>true</legacyModeNS> 
        </plugin>
    </gazebo>
    <!--car's movement controllers end  -->
</robot>

1.2 写配置

ros_ws/src/slam_model/config/ackman_control.yaml里写入：

ackman:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  left_bridge_position_controller:
    type: position_controllers/JointPositionController
    joint: left_bridge_joint
    pid: { p: 20, d: 5.0}

  right_bridge_position_controller:
    type: position_controllers/JointPositionController
    joint: right_bridge_joint
    pid: { p: 20, d: 5.0}

  left_wheel_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: left_rear_wheel_joint
    pid: { p: 100, i: 0.1, d: 10.0}
    
  right_wheel_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: right_rear_wheel_joint
    pid: { p: 30, i: 0.1, d: 10.0}

1.3 编写启动文件

这次启动文件改了改，下次直接改俩参数就可以用：

<launch>
    <arg name="model_name" value="ackman"/>
    <arg name="model_ns" value="ackman"/>
    <include file="$(find gazebo_ros)/launch/empty_world.launch"></include>
    <param name="robot_description" textfile="$(find slam_model)/urdf/$(arg model_name).urdf" />
    <param name="use_gui" value="true"/>   
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
        <remap from="/joint_states" to="/$(arg model_ns)/joint_states" />
    </node>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_model)/urdf/$(arg model_name).rviz" required="true"></node>
    <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find slam_model)/urdf/$(arg model_name).urdf -urdf -model $(find slam_model)" output="screen"/>
    <rosparam file="$(find slam_model)/config/$(arg model_name)_control.yaml" command="load"/>
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
        output="screen" ns="/$(arg model_ns)" args="joint_state_controller right_wheel_velocity_controller left_wheel_velocity_controller right_bridge_position_controller left_bridge_position_controller"/>
</launch>

1.4 编写控制节点

创建一个包：catkin_create_pkg slam_keyboard_move roscpp std_msgs message_generation message_runtime
然后写入一个简单点儿的节点，如果你想要松耦合的话，自己把参数加到argv里，然后在launch里加参数：

#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "iostream"
using namespace std;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "cin_control");
  ros::NodeHandle h;
  std_msgs::Float64 left_pos, right_pos, left_vel, right_vel;
  float a,b,c,d;
  ros::Publisher left_pos_pub = h.advertise<std_msgs::Float64>("/ackman/left_bridge_position_controller/command", 10);
  ros::Publisher right_pos_pub = h.advertise<std_msgs::Float64>("/ackman/right_bridge_position_controller/command", 10);
  ros::Publisher left_vel_pub = h.advertise<std_msgs::Float64>("/ackman/left_wheel_velocity_controller/command", 10);
  ros::Publisher right_vel_pub = h.advertise<std_msgs::Float64>("/ackman/right_wheel_velocity_controller/command", 10);
  ros::Rate loop_rate(1);
  while (ros::ok())
  {
    cout << "please input ackman's info like left_pos right_pos left_vel right_vel" << endl;
    cin >> left_pos.data >> right_pos.data >> left_vel.data >> right_vel.data;
    left_pos_pub.publish(left_pos);
    right_pos_pub.publish(right_pos);
    left_vel_pub.publish(left_vel);
    right_vel_pub.publish(right_vel);
    ROS_INFO("ackman's vel msg send!");
  }
  return 0;
}

修改配置，运行：
在这里插入图片描述
注意：如果给前轮加上了速度控制器，那么前轮会锁定为0m/s，那么前轮就不是从动轮了。但是如果不加的话，前轮和前转轴的关系节点又不会发送到rviz里，所以感觉怪怪的。不过Gazebo里可以就行了，rviz里只用来看数据，所以不重要

二.升级车

使用Gazebo plugins为车创建Lidar、IMU、Camera。文档地址：http://gazebosim.org/tutorials?tut=ros_gzplugins

2.1 添加雷达

向urdf里添加这些东西:

    <!--Lidar begin-->
    <joint name="lidar_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0.1"/>
        <parent link="base_link"/>
        <child link="hokuyo_lidar"/>
    </joint>

    <link name="hokuyo_lidar">
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.05" length="0.1"/>
			</geometry>
			<material name="yellow">
				<color rgba="0.5 1 0 1"/>
		  	</material>
		</visual>
	</link>

    <gazebo reference="hokuyo_lidar">
        <sensor type="gpu_ray" name="head_hokuyo_sensor">
            <pose>0 0 0 0 0 0</pose>
            <visualize>false</visualize>
            <update_rate>60</update_rate>
            <ray>
            <scan>
                <horizontal>
                <samples>720</samples>
                <resolution>1</resolution>
                <min_angle>-1.570796</min_angle>
                <max_angle>1.570796</max_angle>
                </horizontal>
            </scan>
            <range>
                <min>0.10</min>
                <max>10.0</max>
                <resolution>0.01</resolution>
            </range>
            <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.01</stddev>
            </noise>
            </ray>
            <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">
            <topicName>/ackman/laser/scan</topicName>
            <frameName>hokuyo_lidar</frameName>
            </plugin>
        </sensor>
        <material>Gazebo/Yellow</material>
    </gazebo>
    <!--Lidar End-->

这些参数看一眼就懂，不懂的去官网看，我们唯一要修改的是：<topicName>/ackman/laser/scan</topicName>，这是你激光数据的话题信息。
然后用launch启动环境，在rviz里添加LaserScan和配置激光话题信息
在这里插入图片描述

2.2 Camera

在urdf里添加:

    <!--Camera Begin-->
    <link name="camera_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.01 0.05 0.03"/>
            </geometry>
            <material name="green">
				<color rgba="0 1 1 0.8"/>
		  	</material>
        </visual>
    </link>

    <joint name="camera_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0.395 0 0.065"/>
        <parent link="base_link"/>
        <child link="camera_link"/>
    </joint>

    <gazebo reference="camera_link">
        <sensor type="camera" name="camera1">
            <update_rate>30.0</update_rate>
            <camera name="head">
                <horizontal_fov>1.3962634</horizontal_fov>
                <image>
                    <width>800</width>
                    <height>800</height>
                    <format>R8G8B8</format>
                </image>
                <clip>
                    <near>0.02</near>
                    <far>300</far>
                </clip>
                <noise>
                    <type>gaussian</type>
                    <mean>0.0</mean>
                    <stddev>0.007</stddev>
                </noise>
            </camera>
            <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>0.0</updateRate>
                <cameraName>ackman/camera1</cameraName>
                <imageTopicName>image_raw</imageTopicName>
                <cameraInfoTopicName>camera_info</cameraInfoTopicName>
                <frameName>camera_link</frameName>
                <hackBaseline>0.07</hackBaseline>
                <distortionK1>0.0</distortionK1>
                <distortionK2>0.0</distortionK2>
                <distortionK3>0.0</distortionK3>
                <distortionT1>0.0</distortionT1>
                <distortionT2>0.0</distortionT2>
            </plugin>
        </sensor>
        <material>Gazebo/Green</material>
    </gazebo>
    <!--Camera End-->

含义不多说，你的raw格式的相机话题是：<cameraName>ackman/camera1</cameraName> + <imageTopicName>image_raw</imageTopicName> 即：/ackman/camera1/image_raw

在这里插入图片描述

2.3 IMU

老样子，写：

    <!--IMU Begin-->
        <link name="imu_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.06 0.06 0.02"/>
            </geometry>
            <material name="oriange">
				<color rgba="1 0.7 0 0.8"/>
		  	</material>
        </visual>
    </link>

    <joint name="imu_joint" type="fixed">
        <origin rpy="0 0 0" xyz="-0.2 0 0.06"/>
        <parent link="base_link"/>
        <child link="imu_link"/>
    </joint>

    <gazebo reference="imu_link">
        <gravity>false</gravity>
        <sensor name="imu_sensor" type="imu">
            <always_on>true</always_on>
            <update_rate>6o</update_rate>
            <visualize>true</visualize>
            <topic>__default_topic__</topic>
            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
                <topicName>imu</topicName>
                <bodyName>imu_link</bodyName>
                <updateRateHZ>10.0</updateRateHZ>
                <gaussianNoise>0.0</gaussianNoise>
                <xyzOffset>0 0 0</xyzOffset>
                <rpyOffset>0 0 0</rpyOffset>
                <frameName>imu_link</frameName>
                <initialOrientationAsReference>false</initialOrientationAsReference>
            </plugin>
            <pose>0 0 0 0 0 0</pose>
        </sensor>
        <material>Gazebo/Oriange</material>
    </gazebo>
    <!--IMU End-->

看：
在这里插入图片描述

三.整个urdf的代码

<?xml version="1.0" ?>
<robot name="ackman">  
    <!--Main Body Begin-->
    <link name="base_footprint">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
            <box size="0.001 0.001 0.001"/>
            </geometry>
        </visual>
    </link>

    <joint name="base_footprint_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0.2"/>
        <parent link="base_footprint"/>
        <child link="base_link"/>
    </joint>

    <link name="base_link">
        <inertial>
            <origin xyz="0 0 0"/>
            <mass value="300"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>
        <visual>
            <geometry>
                <box size="0.8 0.4 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="blue">
                <color rgba="0.5 0.2 0.8 1"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.8 0.4 0.1"/>
            </geometry>
            <inertial>
                <mass value="300"/>
                <inertia ixx="0.0053" ixy="0" ixz="0" iyy="0.0083" iyz="0" izz="0.0667"/>
            </inertial>
        </collision>
    </link>

    <link name="base_wheel">
        <inertial>
            <origin xyz="0 0 0"/>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
        <visual>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="blue">
                <color rgba="0.5 0.2 0.8 1"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.001 0.001 0.001"/>
            </geometry>
            <inertial>
                <mass value="0.1"/>
                <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
            </inertial>
        </collision>
    </link>

    <joint name="base_wheel_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 -0.1"/>
        <parent link="base_link"/>
        <child link="base_wheel"/>
    </joint>

	<joint name="left_rear_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="base_wheel"/>
		<child link="left_rear_wheel"/>
		<origin rpy="1.57 0 0" xyz="-0.3 0.3 0"/>
		<limit effort="10" velocity="100"/>
		<joint_properties damping="0.0" friction="0.0"/>    
	</joint>

    <link name="left_rear_wheel">
        <inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
            </inertial>
		</collision>
	</link>

    <joint name="right_rear_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="base_wheel"/>
		<child link="right_rear_wheel"/>
		<origin rpy="1.57 0 0" xyz="-0.3 -0.3 0"/>
		<limit effort="10" velocity="100"/>
		<joint_properties damping="0.0" friction="0.0"/>    
	</joint>

    <link name="right_rear_wheel">
        <inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-0.05" ixy="0.0" ixz="0.0" iyy="-0.05" iyz="0.0" izz="-0.05"/>
            </inertial>
		</collision>
	</link>
    
    <joint name="left_bridge_joint" type="revolute">
        <parent link="base_wheel"/>
        <child link="left_bridge"/>
        <origin rpy="0 0 0" xyz="0.3 0.255 0"/>  <!--Y=0.25-0.5*whell_length-0.5*brige_y-->
        <axis xyz="0 0 1"/>
        <limit effort="2.5" lower="-0.758" upper="0.758" velocity="1"/>
    </joint>

    <link name="left_bridge">
        <inertial>
            <origin xyz="0.0 0 0"/>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
        <visual>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <material name="red">
            <color rgba="1 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <inertial>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
        </collision>
    </link>

    <joint name="left_front_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="left_bridge"/>
		<child link="left_front_wheel"/>
		<origin rpy="1.57 0 0" xyz="0 0.045 0"/> <!--Z=0.5*whell_length+0.5*brige_y-->
	</joint>

    <link name="left_front_wheel">
		<inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
		</collision>
	</link>

    <!--right bridge and wheel are mirrored from left-->
    <joint name="right_bridge_joint" type="revolute">
        <parent link="base_wheel"/>
        <child link="right_bridge"/>
        <origin rpy="0 0 0" xyz="0.3 -0.255 0"/>  <!--Y=0.25-0.5*whell_length-0.5*brige_y-->
        <axis xyz="0 0 1"/>
        <limit effort="2.5" lower="-0.758" upper="0.758" velocity="1"/>
    </joint>

    <link name="right_bridge">
        <inertial>
            <origin xyz="0.0 0 0"/>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
        </inertial>
        <visual>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <material name="red">
            <color rgba="1 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
            <box size="0.06 0.04 0.01"/>
            </geometry>
            <inertial>
            <mass value="0.1"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
        </collision>
    </link>

    <joint name="right_front_wheel_joint" type="continuous">
		<axis xyz="0 0 -1"/>
		<parent link="right_bridge"/>
		<child link="right_front_wheel"/>
		<origin rpy="1.57 0 0" xyz="0 -0.045 0"/> <!--Z=0.5*whell_length+0.5*brige_y-->
	</joint>

    <link name="right_front_wheel">
		<inertial>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <mass value="10"/>
            <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            <cylinder radius="0.1" length="0.05"/>
        </inertial>
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.1" length="0.05"/>
			</geometry>
			<material name="black">
				<color rgba="0 0 0 1"/>
		  	</material>
		</visual>
		<collision>
			<origin rpy="0 0 0" xyz="0 0 0"/>
		  	<geometry>
		  		<cylinder radius="0.1" length="0.05"/>
		  	</geometry>
            <inertial>
                <mass value="10"/>
                <inertia ixx="-1.0" ixy="0.0" ixz="0.0" iyy="-1.0" iyz="0.0" izz="-1.0"/>
            </inertial>
		</collision>
	</link>

    <gazebo reference="base_link">
        <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="left_rear_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="right_rear_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="left_front_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="right_front_wheel">
        <material>Gazebo/FlatBlack</material>
    </gazebo>
    <gazebo reference="left_bridge">
        <material>Gazebo/Red</material>
    </gazebo>
    <gazebo reference="right_bridge">
        <material>Gazebo/Red</material>
    </gazebo>
    <!--Main Body End-->

    <!--car's movement controllers begin-->
    <transmission name="right_wheel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="right_rear_wheel_joint">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="right_wheel_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <transmission name="left_wheel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="left_rear_wheel_joint">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="left_wheel_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    
    <!-- dont add these controller into urdf, or you will find a funny movement -->
    <!-- car's front wheel will keep 0m/s because of the controller
    <transmission name="right_front_wheel">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="right_front_wheel_joint">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        </joint>
        <actuator name="right_front_wheel_motor">
            <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    -->
    <transmission name="right_bridge">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="right_bridge_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="right_bridge_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <transmission name="left_bridge">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="left_bridge_joint">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="left_bridge_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>

    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="ros_control">
            <robotNamespace>/ackman</robotNamespace>
            <legacyModeNS>true</legacyModeNS> 
        </plugin>
    </gazebo>
    <!--car's movement controllers end  -->
    
    <!--Lidar begin-->
    <joint name="lidar_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0.1"/>
        <parent link="base_link"/>
        <child link="hokuyo_lidar"/>
    </joint>

    <link name="hokuyo_lidar">
		<visual>
			<origin rpy="0 0 0" xyz="0 0 0"/>
			<geometry>
				<cylinder radius="0.05" length="0.1"/>
			</geometry>
			<material name="yellow">
				<color rgba="0.5 1 0 1"/>
		  	</material>
		</visual>
	</link>

    <gazebo reference="hokuyo_lidar">
        <sensor type="gpu_ray" name="head_hokuyo_sensor">
            <pose>0 0 0 0 0 0</pose>
            <visualize>false</visualize>
            <update_rate>60</update_rate>
            <ray>
            <scan>
                <horizontal>
                <samples>720</samples>
                <resolution>1</resolution>
                <min_angle>-1.570796</min_angle>
                <max_angle>1.570796</max_angle>
                </horizontal>
            </scan>
            <range>
                <min>0.10</min>
                <max>10.0</max>
                <resolution>0.01</resolution>
            </range>
            <noise>
                <type>gaussian</type>
                <mean>0.0</mean>
                <stddev>0.01</stddev>
            </noise>
            </ray>
            <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">
            <topicName>/ackman/laser/scan</topicName>
            <frameName>hokuyo_lidar</frameName>
            </plugin>
        </sensor>
        <material>Gazebo/Yellow</material>
    </gazebo>
    <!--Lidar End-->

    <!--Camera Begin-->
    <link name="camera_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.01 0.05 0.03"/>
            </geometry>
            <material name="green">
				<color rgba="0 1 1 0.8"/>
		  	</material>
        </visual>
    </link>

    <joint name="camera_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0.395 0 0.065"/>
        <parent link="base_link"/>
        <child link="camera_link"/>
    </joint>

    <gazebo reference="camera_link">
        <sensor type="camera" name="camera1">
            <update_rate>30.0</update_rate>
            <camera name="head">
                <horizontal_fov>1.3962634</horizontal_fov>
                <image>
                    <width>800</width>
                    <height>800</height>
                    <format>R8G8B8</format>
                </image>
                <clip>
                    <near>0.02</near>
                    <far>300</far>
                </clip>
                <noise>
                    <type>gaussian</type>
                    <mean>0.0</mean>
                    <stddev>0.007</stddev>
                </noise>
            </camera>
            <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
                <alwaysOn>true</alwaysOn>
                <updateRate>0.0</updateRate>
                <cameraName>ackman/camera1</cameraName>
                <imageTopicName>image_raw</imageTopicName>
                <cameraInfoTopicName>camera_info</cameraInfoTopicName>
                <frameName>camera_link</frameName>
                <hackBaseline>0.07</hackBaseline>
                <distortionK1>0.0</distortionK1>
                <distortionK2>0.0</distortionK2>
                <distortionK3>0.0</distortionK3>
                <distortionT1>0.0</distortionT1>
                <distortionT2>0.0</distortionT2>
            </plugin>
        </sensor>
        <material>Gazebo/Green</material>
    </gazebo>
    <!--Camera End-->

    <!--IMU Begin-->
        <link name="imu_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <box size="0.06 0.06 0.02"/>
            </geometry>
            <material name="oriange">
				<color rgba="1 0.7 0 0.8"/>
		  	</material>
        </visual>
    </link>

    <joint name="imu_joint" type="fixed">
        <origin rpy="0 0 0" xyz="-0.2 0 0.06"/>
        <parent link="base_link"/>
        <child link="imu_link"/>
    </joint>

    <gazebo reference="imu_link">
        <gravity>false</gravity>
        <sensor name="imu_sensor" type="imu">
            <always_on>true</always_on>
            <update_rate>6o</update_rate>
            <visualize>true</visualize>
            <topic>__default_topic__</topic>
            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
                <topicName>imu</topicName>
                <bodyName>imu_link</bodyName>
                <updateRateHZ>10.0</updateRateHZ>
                <gaussianNoise>0.0</gaussianNoise>
                <xyzOffset>0 0 0</xyzOffset>
                <rpyOffset>0 0 0</rpyOffset>
                <frameName>imu_link</frameName>
                <initialOrientationAsReference>false</initialOrientationAsReference>
            </plugin>
            <pose>0 0 0 0 0 0</pose>
        </sensor>
        <material>Gazebo/Oriange</material>
    </gazebo>
    <!--IMU End-->
</robot>

四.后记

虽然完成了，但是还是没有搞清楚一些力学限制。给定一个初速度后，会给轮胎一个较大的加速度，那么这个时候，质量、力限制、速度限制、转动惯量可能都对这个有影响。最开始的模型启动是会直接翻车的。但是通过改大质量、改小effort就能稳定，但总感觉不定性的去改他们总感觉怪怪的。

康娜喵
关注
专栏目录
评论 4
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

康娜喵

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值