阿克曼四轮小车运动控制

src/chap7/
├── smartcar_description/          # 小车模型、控制器配置与启动文件
│   ├── config/
│   │   ├── smartcar_joint.yaml    # 五个 ros_control 控制器配置
│   │   └── smartcar.rviz          # Rviz 显示配置（含 LaserScan）
│   ├── launch/xacro/
│   │   └── smartcar_gazebo.launch # 总启动文件
│   └── urdf/xacro/
│       ├── smartcar.xacro         # xacro 入口
│       ├── smartcar_body.xacro    # 车体模型
│       └── smartcar_sim.xacro     # 传感器与控制插件
└── smartcar_control/              # 运动控制节点
    ├── launch/
    │   └── smartcar_gazebo_controller.launch
    ├── scripts/
    │   └── front_wheel_jsp.py     # 前轮关节状态补全节点
    └── src/
        └── smartcar_control_gazebo.cpp  # 键盘遥控与阿克曼解算

<?xml version="1.0"?>
<robot name="smartcar" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find smartcar_description)/urdf/xacro/smartcar_body.xacro"/>
  <xacro:include filename="$(find smartcar_description)/urdf/xacro/smartcar_sim.xacro"/>
  <xacro:smartcar_body/>
  <xacro:smartcar_sim/>
</robot>

<xacro:macro name="cylinder_inertial_matrix" params="m r h">
  <inertial>
    <mass value="${m}" />
    <inertia
      ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
      iyy="${m*(3*r*r+h*h)/12}" iyz="0"
      izz="${m*r*r/2}" />
  </inertial>
</xacro:macro>

<cylinder_inertial_matrix m="0.1" r="0.025" h="0.05"/>

<xacro:cylinder_inertial_matrix m="0.1" r="0.025" h="0.05"/>

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
# 将 smartcar_description、smartcar_control 置于 src/chap7 下
cd ~/catkin_ws
catkin_make
source devel/setup.bash

roslaunch smartcar_description smartcar_gazebo.launch

<include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="gui" value="$(arg gui)"/>
</include>

<param name="robot_description"
       command="$(find xacro)/xacro --inorder '$(find smartcar_description)/urdf/xacro/smartcar.xacro'" />
<rosparam file="$(find smartcar_description)/config/smartcar_joint.yaml" command="load"/>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
      args="-urdf -model mrobot -param robot_description" output="screen"/>

Finished loading Gazebo ROS API Plugin.

rostopic list

<node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
      args="joint_state_controller
            rear_right_velocity_controller
            rear_left_velocity_controller
            right_bridge_position_controller
            left_bridge_position_controller"/>

Loaded **joint_state_controller**
Started ...
Loaded **rear_right_velocity_controller**
Loaded **rear_left_velocity_controller**
Loaded **right_bridge_position_controller**
Loaded **left_bridge_position_controller**

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import JointState

rospy.init_node('front_wheel_jsp')
pub = rospy.Publisher('/joint_states', JointState, queue_size=10)
rate = rospy.Rate(50)

msg = JointState()
msg.name = ['right_front_wheel_to_bridge', 'left_front_wheel_to_bridge']
msg.position = [0.0, 0.0]
msg.velocity = [0.0, 0.0]

while not rospy.is_shutdown():
    msg.header.stamp = rospy.Time.now()
    pub.publish(msg)
    rate.sleep()

rosrun smartcar_control front_wheel_jsp.py

rviz -d $(rospack find smartcar_description)/config/smartcar.rviz

joint_state_controller:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50

rear_right_velocity_controller:
  type: "velocity_controllers/JointVelocityController"
  joint: right_back_wheel_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
rear_left_velocity_controller:
  type: "velocity_controllers/JointVelocityController"
  joint: left_back_wheel_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
right_bridge_position_controller:
  type: "effort_controllers/JointPositionController"
  joint: right_bridge_to_bridge
  pid: {p: 40.0, i: 0.0, d: 1.0}
left_bridge_position_controller:
  type: "effort_controllers/JointPositionController"
  joint: left_bridge_to_bridge
  pid: {p: 40.0, i: 0.0, d: 1.0}

outer_angle = atan(wheelbase * tan(angle) / (wheelbase + 0.5 * width  * tan(angle)))
inner_angle = atan(wheelbase * tan(angle) / (wheelbase - 0.5 * width  * tan(angle)))

outside_speed = speed * (1 + 0.5 * width * tan(angle) / wheelbase)
inside_speed  = speed * (1 - 0.5 * width * tan(angle) / wheelbase)

roslaunch smartcar_control smartcar_gazebo_controller.launch

<sensor type="ray" name="head_hokuyo_sensor">
  <visualize>true</visualize>
  <ray>
    <scan>
      <horizontal>
        <samples>720</samples>
        <min_angle>-1.570796</min_angle>
        <max_angle>1.570796</max_angle>
      </horizontal>
    </scan>
    <range>
      <min>0.10</min>
      <max>30.0</max>
      <resolution>0.01</resolution>
    </range>
    <noise>
      <type>gaussian</type>
      <stddev>0.01</stddev>
    </noise>
  </ray>
  <plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
    <topicName>/laser/scan</topicName>
    <frameName>laser_link</frameName>
  </plugin>
</sensor>

killall gzserver gzclient
roslaunch smartcar_description smartcar_gazebo.launch

rostopic hz /laser/scan

rqt_image_view /camera/image_raw

rostopic hz /imu

rostopic echo /imu

sed -i '155,156d' ~/catkin_ws/src/chap7/smartcar_control/CMakeLists.txt

cd ~/catkin_ws
catkin_make