Refactor the robot description, and include mecanum control.
This commit is contained in:
Matt Spencer
@@ -11,26 +11,21 @@ controller_manager:
|
|||||||
# ─────────────────────────────────────────────────────────────────────────────
|
# ─────────────────────────────────────────────────────────────────────────────
|
||||||
# Mecanum drive controller
|
# Mecanum drive controller
|
||||||
# ─────────────────────────────────────────────────────────────────────────────
|
# ─────────────────────────────────────────────────────────────────────────────
|
||||||
# NOTE: The URDF joint names do not match the physical wheel positions due to a
|
# Wheel link names match their physical corners:
|
||||||
# naming inconsistency in agv_pro.urdf. The mapping between the controller's
|
# front-left (FL) -> left_front_wheel_joint
|
||||||
# logical positions and the URDF joint names is as follows:
|
# front-right (FR) -> right_front_wheel_joint
|
||||||
#
|
# rear-left (RL) -> left_rear_wheel_joint
|
||||||
# Physical position │ URDF joint name
|
# rear-right (RR) -> right_rear_wheel_joint
|
||||||
# ──────────────────┼────────────────────────────
|
|
||||||
# front-left (FL) │ left_front_wheel_joint ✓
|
|
||||||
# front-right (FR) │ left_rear_wheel_joint ← physically front-right
|
|
||||||
# rear-left (RL) │ right_front_wheel_joint ← physically rear-left
|
|
||||||
# rear-right (RR) │ right_rear_wheel_joint ✓
|
|
||||||
#
|
#
|
||||||
# The same mapping is used by the hardware interface (front_left_joint /
|
# The same mapping is used by the hardware interface (front_left_joint /
|
||||||
# front_right_joint / rear_left_joint / rear_right_joint params in the URDF
|
# front_right_joint / rear_left_joint / rear_right_joint params in the URDF
|
||||||
# <ros2_control> block).
|
# <ros2_control> block).
|
||||||
mecanum_drive_controller:
|
mecanum_drive_controller:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
front_left_wheel_command_joint_name: left_front_wheel_joint
|
front_left_wheel_command_joint_name: front_left_wheel_joint
|
||||||
front_right_wheel_command_joint_name: left_rear_wheel_joint
|
front_right_wheel_command_joint_name: front_right_wheel_joint
|
||||||
rear_left_wheel_command_joint_name: right_front_wheel_joint
|
rear_left_wheel_command_joint_name: rear_left_wheel_joint
|
||||||
rear_right_wheel_command_joint_name: right_rear_wheel_joint
|
rear_right_wheel_command_joint_name: rear_right_wheel_joint
|
||||||
|
|
||||||
odom_frame_id: odom
|
odom_frame_id: odom
|
||||||
base_frame_id: base_footprint
|
base_frame_id: base_footprint
|
||||||
|
|||||||
@@ -2,38 +2,16 @@
|
|||||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
<!--
|
<!--
|
||||||
Gazebo (gz-sim Harmonic) specific tags. Only included when sim=true.
|
Gazebo (gz-sim Harmonic) integration. Only two responsibilities:
|
||||||
Provides wheel surface friction/material and loads the gz_ros2_control
|
1. Load the gz_ros2_control system plugin, which hosts the
|
||||||
system plugin that hosts the controller_manager inside gz-sim.
|
controller_manager inside gz-sim and reads the controller yaml.
|
||||||
|
Per-wheel mecanum friction is emitted by the wheel macro when
|
||||||
|
gazebo_ignition (sim) is true, so it is NOT repeated here.
|
||||||
-->
|
-->
|
||||||
<xacro:macro name="agv_pro_gazebo" params="namespace controllers_file">
|
<xacro:macro name="agv_pro_gazebo" params="prefix controllers_file">
|
||||||
|
|
||||||
<!-- Per-wheel surface friction + dark material -->
|
|
||||||
<xacro:macro name="wheel_gz" params="link">
|
|
||||||
<gazebo reference="${link}">
|
|
||||||
<mu1>0.8</mu1>
|
|
||||||
<mu2>0.8</mu2>
|
|
||||||
<kp>100000.0</kp>
|
|
||||||
<kd>1.0</kd>
|
|
||||||
<visual>
|
|
||||||
<material>
|
|
||||||
<ambient>0.1 0.1 0.1 1</ambient>
|
|
||||||
<diffuse>0.1 0.1 0.1 1</diffuse>
|
|
||||||
<specular>0.1 0.1 0.1 1</specular>
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
</gazebo>
|
|
||||||
</xacro:macro>
|
|
||||||
|
|
||||||
<xacro:wheel_gz link="${namespace}left_front_wheel_link"/>
|
|
||||||
<xacro:wheel_gz link="${namespace}right_front_wheel_link"/>
|
|
||||||
<xacro:wheel_gz link="${namespace}left_rear_wheel_link"/>
|
|
||||||
<xacro:wheel_gz link="${namespace}right_rear_wheel_link"/>
|
|
||||||
|
|
||||||
<!-- gz_ros2_control system plugin: hosts the controller_manager in gz-sim -->
|
|
||||||
<gazebo>
|
<gazebo>
|
||||||
<plugin filename="gz_ros2_control-system"
|
<plugin filename="gz_ros2_control-system"
|
||||||
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
||||||
<parameters>${controllers_file}</parameters>
|
<parameters>${controllers_file}</parameters>
|
||||||
</plugin>
|
</plugin>
|
||||||
</gazebo>
|
</gazebo>
|
||||||
|
|||||||
@@ -1,59 +0,0 @@
|
|||||||
<?xml version="1.0"?>
|
|
||||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
||||||
|
|
||||||
<!--
|
|
||||||
ros2_control block shared by the physical robot and the Gazebo simulation.
|
|
||||||
|
|
||||||
The only difference between the two is the <hardware> plugin:
|
|
||||||
sim=false -> agv_pro_hardware/AgvProHardwareInterface (real serial base)
|
|
||||||
sim=true -> gz_ros2_control/GazeboSimSystem (Gazebo Harmonic)
|
|
||||||
|
|
||||||
Joint-name note: the URDF joint origins show two wheels are physically at the
|
|
||||||
'wrong' position relative to their names (left_rear is physically front-right,
|
|
||||||
right_front is physically rear-left). The hardware plugin (real) and the
|
|
||||||
mecanum_drive_controller (both) resolve this via the front/rear/left/right
|
|
||||||
parameter mapping.
|
|
||||||
-->
|
|
||||||
<xacro:macro name="agv_pro_ros2_control" params="sim namespace port_name">
|
|
||||||
<ros2_control name="AgvProSystem" type="system">
|
|
||||||
<hardware>
|
|
||||||
<xacro:if value="${sim}">
|
|
||||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
|
||||||
</xacro:if>
|
|
||||||
<xacro:unless value="${sim}">
|
|
||||||
<plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
|
|
||||||
<param name="port_name">${port_name}</param>
|
|
||||||
<param name="wheel_radius">0.072</param>
|
|
||||||
<param name="lx">0.172</param>
|
|
||||||
<param name="ly">0.180</param>
|
|
||||||
<!-- Physical wheel position -> URDF joint name mapping -->
|
|
||||||
<param name="front_left_joint">${namespace}left_front_wheel_joint</param>
|
|
||||||
<param name="front_right_joint">${namespace}left_rear_wheel_joint</param>
|
|
||||||
<param name="rear_left_joint">${namespace}right_front_wheel_joint</param>
|
|
||||||
<param name="rear_right_joint">${namespace}right_rear_wheel_joint</param>
|
|
||||||
</xacro:unless>
|
|
||||||
</hardware>
|
|
||||||
|
|
||||||
<joint name="${namespace}left_front_wheel_joint">
|
|
||||||
<command_interface name="velocity"/>
|
|
||||||
<state_interface name="position"/>
|
|
||||||
<state_interface name="velocity"/>
|
|
||||||
</joint>
|
|
||||||
<joint name="${namespace}left_rear_wheel_joint">
|
|
||||||
<command_interface name="velocity"/>
|
|
||||||
<state_interface name="position"/>
|
|
||||||
<state_interface name="velocity"/>
|
|
||||||
</joint>
|
|
||||||
<joint name="${namespace}right_front_wheel_joint">
|
|
||||||
<command_interface name="velocity"/>
|
|
||||||
<state_interface name="position"/>
|
|
||||||
<state_interface name="velocity"/>
|
|
||||||
</joint>
|
|
||||||
<joint name="${namespace}right_rear_wheel_joint">
|
|
||||||
<command_interface name="velocity"/>
|
|
||||||
<state_interface name="position"/>
|
|
||||||
<state_interface name="velocity"/>
|
|
||||||
</joint>
|
|
||||||
</ros2_control>
|
|
||||||
</xacro:macro>
|
|
||||||
</robot>
|
|
||||||
@@ -1,255 +1,152 @@
|
|||||||
<?xml version="1.0" encoding="utf-8"?>
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
<robot name="agv_pro" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
<robot name="agv_pro" xmlns:xacro="http://www.ros.org/wiki/xacro"
|
||||||
|
xmlns:gz="http://gazebosim.org/schema">
|
||||||
|
|
||||||
<!-- Optional namespace prefix for all link/joint names -->
|
<!-- Optional namespace prefix for all link/joint names -->
|
||||||
<xacro:arg name="namespace" default=""/>
|
<xacro:arg name="prefix" default="" />
|
||||||
<xacro:property name="namespace" value="$(arg namespace)"/>
|
<xacro:property name="prefix" value="$(arg prefix)" />
|
||||||
|
|
||||||
<!-- Hardware selection: false -> real serial base, true -> Gazebo (gz-sim) -->
|
<!-- Hardware selection: false -> real serial base, true -> Gazebo (gz-sim) -->
|
||||||
<xacro:arg name="sim" default="false"/>
|
<xacro:arg name="sim" default="false" />
|
||||||
<xacro:property name="sim" value="$(arg sim)"/>
|
<xacro:property name="sim" value="$(arg sim)" />
|
||||||
|
|
||||||
<!-- Real-hardware serial port (ignored when sim=true) -->
|
<!-- Real-hardware serial port (ignored when sim=true) -->
|
||||||
<xacro:arg name="port_name" default="/dev/agvpro_controller"/>
|
<xacro:arg name="port_name" default="/dev/agvpro_controller" />
|
||||||
<xacro:property name="port_name" value="$(arg port_name)"/>
|
<xacro:property name="port_name" value="$(arg port_name)" />
|
||||||
|
|
||||||
<!-- Controller params file consumed by gz_ros2_control (sim only) -->
|
<!-- Controller params file consumed by gz_ros2_control (sim only) -->
|
||||||
<xacro:arg name="controllers_file" default=""/>
|
<xacro:arg name="controllers_file" default="" />
|
||||||
<xacro:property name="controllers_file" value="$(arg controllers_file)"/>
|
<xacro:property name="controllers_file" value="$(arg controllers_file)" />
|
||||||
|
|
||||||
<link name="${namespace}base_footprint"/>
|
<link name="${prefix}base_footprint" />
|
||||||
|
|
||||||
<joint name="${namespace}base_joint" type="fixed">
|
<joint name="${prefix}base_joint" type="fixed">
|
||||||
<parent link="${namespace}base_footprint"/>
|
<parent link="${prefix}base_footprint" />
|
||||||
<child link="${namespace}base_link" />
|
<child link="${prefix}base_link" />
|
||||||
<origin xyz="0 0 0.020" rpy="0 0 0"/>
|
<origin xyz="0 0 0.020" rpy="0 0 0" />
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<link name="${namespace}base_link">
|
<link name="${prefix}base_link">
|
||||||
<inertial>
|
<inertial>
|
||||||
<origin xyz="-0.0076254 -0.00023134 0.06693" rpy="0 0 0" />
|
<origin xyz="-0.0076254 0.0 0.06693" rpy="0 0 0" />
|
||||||
<mass value="19.236" />
|
<mass value="19.236" />
|
||||||
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
|
<inertia ixx="0.14436" ixy="0.0012037" ixz="0.0019458"
|
||||||
iyy="0.24191" iyz="0.0044629"
|
iyy="0.24191" iyz="0.0044629"
|
||||||
izz="0.33755" />
|
izz="0.33755" />
|
||||||
</inertial>
|
</inertial>
|
||||||
|
|
||||||
<visual>
|
<visual>
|
||||||
<origin xyz="0 0 0"
|
<origin xyz="0 0 0"
|
||||||
rpy="0 0 0" />
|
rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="package://agv_pro_description/meshes/base_link.stl" />
|
<mesh filename="package://agv_pro_description/meshes/base_link.stl" />
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="">
|
<material name="">
|
||||||
<color rgba="1 1 1 1" />
|
<color rgba="1 1 1 1" />
|
||||||
</material>
|
</material>
|
||||||
</visual>
|
</visual>
|
||||||
|
|
||||||
<collision>
|
<collision>
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="package://agv_pro_description/meshes/base_link.stl" />
|
<mesh filename="package://agv_pro_description/meshes/base_link.stl" />
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</link>
|
||||||
|
|
||||||
<link name="${namespace}right_rear_wheel_link">
|
<!-- Wheels (built from the shared wheel macro) -->
|
||||||
<inertial>
|
<xacro:include filename="$(find agv_pro_description)/urdf/parts/wheel_macro.xacro" />
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
||||||
<mass value="0.21659" />
|
|
||||||
<inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
|
||||||
iyy="0.00097519" iyz="-2.3635E-07"
|
|
||||||
izz="0.00051178" />
|
|
||||||
</inertial>
|
|
||||||
|
|
||||||
<visual>
|
<xacro:agv_pro_wheel
|
||||||
<origin xyz="0 0 0"
|
prefix="${prefix}front_right_"
|
||||||
rpy="0 0 0" />
|
parent="${prefix}base_link"
|
||||||
<geometry>
|
reflect="false"
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
|
mesh="wheel_lb_link.stl"
|
||||||
</geometry>
|
origin_xyz="0.17128 -0.1799 0.052"
|
||||||
<material name="">
|
gazebo_ignition="${sim}" />
|
||||||
<color rgba="1 1 1 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
|
|
||||||
<collision>
|
<xacro:agv_pro_wheel
|
||||||
<origin xyz="0 0 0"
|
prefix="${prefix}front_left_"
|
||||||
rpy="0 0 0" />
|
parent="${prefix}base_link"
|
||||||
<geometry>
|
reflect="true"
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_rb_link.stl" />
|
mesh="wheel_lf_link.stl"
|
||||||
</geometry>
|
origin_xyz="0.17128 0.1799 0.052"
|
||||||
</collision>
|
gazebo_ignition="${sim}" />
|
||||||
</link>
|
|
||||||
|
|
||||||
<joint name="${namespace}right_rear_wheel_joint" type="continuous">
|
<xacro:agv_pro_wheel
|
||||||
<origin xyz="-0.171806101587598 -0.179900399999999 0.0518836514526621" rpy="0 0 0" />
|
prefix="${prefix}rear_left_"
|
||||||
<parent link="${namespace}base_link" />
|
parent="${prefix}base_link"
|
||||||
<child link="${namespace}right_rear_wheel_link" />
|
reflect="false"
|
||||||
<axis xyz="0 1 0" />
|
mesh="wheel_rf_link.stl"
|
||||||
</joint>
|
origin_xyz="-0.17181 0.1799 0.052"
|
||||||
|
gazebo_ignition="${sim}" />
|
||||||
|
|
||||||
<link name="${namespace}right_front_wheel_link">
|
<xacro:agv_pro_wheel
|
||||||
<inertial>
|
prefix="${prefix}rear_right_"
|
||||||
<origin xyz="6.6563E-05 -0.019725 8.3836E-05" rpy="0 0 0" />
|
parent="${prefix}base_link"
|
||||||
<mass value="0.21659122149244" />
|
reflect="true"
|
||||||
<inertia ixx="0.00051181" ixy="2.1577E-08" ixz="2.538E-07"
|
mesh="wheel_rb_link.stl"
|
||||||
iyy="0.00097519" iyz="-2.3635E-07"
|
origin_xyz="-0.171806101587598 -0.179900399999999 0.052"
|
||||||
izz="0.00051178" />
|
gazebo_ignition="${sim}" />
|
||||||
</inertial>
|
|
||||||
|
|
||||||
<visual>
|
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
|
|
||||||
</geometry>
|
|
||||||
<material name="">
|
|
||||||
<color rgba="1 1 1 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
|
|
||||||
<collision>
|
<link name="${prefix}laser_link">
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<inertial>
|
||||||
<geometry>
|
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_rf_link.stl" />
|
<mass value="0.049095" />
|
||||||
</geometry>
|
<inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05"
|
||||||
</collision>
|
iyz="-4.2154E-09"
|
||||||
</link>
|
izz="3.4612E-05" />
|
||||||
|
</inertial>
|
||||||
|
|
||||||
<joint name="${namespace}right_front_wheel_joint" type="continuous">
|
<visual>
|
||||||
<origin xyz="-0.17181 0.1799 0.051884"
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
rpy="0 0 0" />
|
<geometry>
|
||||||
<parent link="${namespace}base_link" />
|
<mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
|
||||||
<child link="${namespace}right_front_wheel_link" />
|
</geometry>
|
||||||
<axis xyz="0 1 0" />
|
<material name="">
|
||||||
</joint>
|
<color rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
<link name="${namespace}left_front_wheel_link">
|
<collision>
|
||||||
<inertial>
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
<origin xyz="1.4671E-06 -0.019803 4.3218E-06" rpy="0 0 0" />
|
<geometry>
|
||||||
<mass value="0.3015" />
|
<mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
|
||||||
<inertia ixx="0.00052475" ixy="-2.2533E-07" ixz="-4.1904E-07"
|
</geometry>
|
||||||
iyy="0.00099948" iyz="7.5332E-08"
|
</collision>
|
||||||
izz="0.00052362" />
|
</link>
|
||||||
</inertial>
|
|
||||||
|
|
||||||
<visual>
|
<joint name="${prefix}lidar_joint" type="fixed">
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
|
||||||
<geometry>
|
<parent link="${prefix}base_link" />
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
|
<child link="${prefix}laser_link" />
|
||||||
</geometry>
|
</joint>
|
||||||
<material name="">
|
|
||||||
<color rgba="1 1 1 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
|
|
||||||
<collision>
|
<link name="${prefix}camera_link" />
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_lf_link.stl" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<joint name="${namespace}left_front_wheel_joint" type="continuous">
|
<joint name="${prefix}camera_joint" type="fixed">
|
||||||
<origin xyz="0.17128 0.1799 0.052"
|
<origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
|
||||||
rpy="0 0 0" />
|
<parent link="${prefix}base_link" />
|
||||||
<parent link="${namespace}base_link" />
|
<child link="${prefix}camera_link" />
|
||||||
<child link="${namespace}left_front_wheel_link" />
|
</joint>
|
||||||
<axis xyz="0 1 0" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<link name="${namespace}left_rear_wheel_link">
|
<link name="${prefix}imu_link" />
|
||||||
<inertial>
|
|
||||||
<origin xyz="-2.4454E-06 0.019725 -4.3121E-06" rpy="0 0 0" />
|
|
||||||
<mass value="0.29613" />
|
|
||||||
<inertia ixx="0.00051227" ixy="-2.0628E-07" ixz="-4.9074E-07"
|
|
||||||
iyy="0.0009752" iyz="1.173E-07"
|
|
||||||
izz="0.00051131" />
|
|
||||||
</inertial>
|
|
||||||
|
|
||||||
<visual>
|
<joint name="${prefix}imu_joint" type="fixed">
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
|
||||||
<geometry>
|
<parent link="${prefix}base_link" />
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
|
<child link="${prefix}imu_link" />
|
||||||
</geometry>
|
</joint>
|
||||||
<material name="">
|
|
||||||
<color rgba="1 1 1 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
|
|
||||||
<collision>
|
<!-- ros2_control: real hardware or gz_ros2_control depending on 'sim' -->
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
<xacro:include filename="$(find agv_pro_description)/urdf/control/agv_pro.ros2_control.xacro" />
|
||||||
<geometry>
|
<xacro:agv_pro_ros2_control sim="${sim}" prefix="${prefix}" port_name="${port_name}" />
|
||||||
<mesh filename="package://agv_pro_description/meshes/wheel_lb_link.stl" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<joint name="${namespace}left_rear_wheel_joint" type="continuous">
|
<!-- Gazebo system plugin (hosts controller_manager); wheel friction is in the wheel macro -->
|
||||||
<origin xyz="0.17128 -0.1799 0.052" rpy="0 0 0" />
|
<xacro:if value="${sim}">
|
||||||
<parent link="${namespace}base_link" />
|
<xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.gazebo.xacro" />
|
||||||
<child link="${namespace}left_rear_wheel_link" />
|
<xacro:agv_pro_gazebo prefix="${prefix}" controllers_file="${controllers_file}" />
|
||||||
<axis xyz="0 1 0" />
|
</xacro:if>
|
||||||
</joint>
|
|
||||||
|
|
||||||
<link name="${namespace}laser_link">
|
|
||||||
<inertial>
|
|
||||||
<origin xyz="-0.0035142 -2.8248E-05 0.0010013" rpy="0 0 0" />
|
|
||||||
<mass value="0.049095" />
|
|
||||||
<inertia ixx="2.0572E-05" ixy="8.5013E-08" ixz="2.0871E-07" iyy="2.0483E-05"
|
|
||||||
iyz="-4.2154E-09"
|
|
||||||
izz="3.4612E-05" />
|
|
||||||
</inertial>
|
|
||||||
|
|
||||||
<visual>
|
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
|
|
||||||
</geometry>
|
|
||||||
<material name="">
|
|
||||||
<color rgba="1 1 1 1" />
|
|
||||||
</material>
|
|
||||||
</visual>
|
|
||||||
|
|
||||||
<collision>
|
|
||||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
||||||
<geometry>
|
|
||||||
<mesh filename="package://agv_pro_description/meshes/laser_link.stl" />
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<joint name="${namespace}lidar_joint" type="fixed">
|
|
||||||
<origin xyz="0.17891 0 0.20928" rpy="0 0 0" />
|
|
||||||
<parent link="${namespace}base_link" />
|
|
||||||
<child link="${namespace}laser_link" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<link name="${namespace}camera_link"/>
|
|
||||||
|
|
||||||
<joint name="${namespace}camera_joint" type="fixed">
|
|
||||||
<origin xyz="0.23191 0 0.14928" rpy="0 0 0" />
|
|
||||||
<parent link="${namespace}base_link" />
|
|
||||||
<child link="${namespace}camera_link" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<link name="${namespace}imu_link"/>
|
|
||||||
|
|
||||||
<joint name="${namespace}imu_joint" type="fixed">
|
|
||||||
<origin xyz="-0.17181 -0.0270532 0.14928" rpy="0 0 1.5707" />
|
|
||||||
<parent link="${namespace}base_link" />
|
|
||||||
<child link="${namespace}imu_link" />
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<!-- ros2_control: real hardware or gz_ros2_control depending on 'sim' -->
|
|
||||||
<xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.ros2_control.xacro"/>
|
|
||||||
<xacro:agv_pro_ros2_control sim="${sim}" namespace="${namespace}" port_name="${port_name}"/>
|
|
||||||
|
|
||||||
<!-- Gazebo-only tags (friction, material, gz_ros2_control system plugin) -->
|
|
||||||
<xacro:if value="${sim}">
|
|
||||||
<xacro:include filename="$(find agv_pro_description)/urdf/agv_pro.gazebo.xacro"/>
|
|
||||||
<xacro:agv_pro_gazebo namespace="${namespace}" controllers_file="${controllers_file}"/>
|
|
||||||
</xacro:if>
|
|
||||||
|
|
||||||
</robot>
|
</robot>
|
||||||
@@ -0,0 +1,55 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Single ros2_control block, shared by the real robot and the Gazebo sim.
|
||||||
|
Only the <hardware> plugin differs:
|
||||||
|
sim=false -> agv_pro_hardware/AgvProHardwareInterface (real serial base)
|
||||||
|
sim=true -> gz_ros2_control/GazeboSimSystem (Gazebo Harmonic)
|
||||||
|
|
||||||
|
Joint names match the wheel macro:
|
||||||
|
<prefix>{front_left,front_right,rear_left,rear_right}_wheel_joint
|
||||||
|
Each wheel sits at its true corner, so the mapping is 1:1 (no swaps).
|
||||||
|
-->
|
||||||
|
<xacro:macro name="agv_pro_ros2_control" params="sim prefix port_name">
|
||||||
|
<ros2_control name="AgvProSystem" type="system">
|
||||||
|
<hardware>
|
||||||
|
<xacro:if value="${sim}">
|
||||||
|
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||||
|
</xacro:if>
|
||||||
|
<xacro:unless value="${sim}">
|
||||||
|
<plugin>agv_pro_hardware/AgvProHardwareInterface</plugin>
|
||||||
|
<param name="port_name">${port_name}</param>
|
||||||
|
<param name="wheel_radius">0.072</param>
|
||||||
|
<param name="lx">0.172</param>
|
||||||
|
<param name="ly">0.180</param>
|
||||||
|
<param name="front_left_joint">${prefix}front_left_wheel_joint</param>
|
||||||
|
<param name="front_right_joint">${prefix}front_right_wheel_joint</param>
|
||||||
|
<param name="rear_left_joint">${prefix}rear_left_wheel_joint</param>
|
||||||
|
<param name="rear_right_joint">${prefix}rear_right_wheel_joint</param>
|
||||||
|
</xacro:unless>
|
||||||
|
</hardware>
|
||||||
|
|
||||||
|
<joint name="${prefix}front_left_wheel_joint">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}front_right_wheel_joint">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}rear_left_wheel_joint">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}rear_right_wheel_joint">
|
||||||
|
<command_interface name="velocity"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
@@ -0,0 +1,59 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro">
|
||||||
|
<!-- code from https://github.com/RobotnikAutomation/robotnik_description/blob/jazzy-devel/urdf/inertia.urdf.xacro -->
|
||||||
|
<!-- source en.wikipedia.org/wiki/List_of_moments_of_inertia-->
|
||||||
|
|
||||||
|
<!-- TODO Solid sphere of radius r and mass m-->
|
||||||
|
<xacro:macro
|
||||||
|
name="solid_sphere_inertia"
|
||||||
|
params="m r">
|
||||||
|
<inertia
|
||||||
|
ixx="${(2*r*r*m)/5}"
|
||||||
|
ixy="0"
|
||||||
|
ixz="0"
|
||||||
|
iyy="${(2*r*r*m)/5}"
|
||||||
|
iyz="0"
|
||||||
|
izz="${(2*r*r*m)/5}" />
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
<!-- TODO Hollow sphere of radius r and mass m-->
|
||||||
|
<xacro:macro
|
||||||
|
name="hollow_sphere_inertia"
|
||||||
|
params="m r">
|
||||||
|
<inertia
|
||||||
|
ixx="${(2*r*r*m)/3}"
|
||||||
|
ixy="0"
|
||||||
|
ixz="0"
|
||||||
|
iyy="${(2*r*r*m)/3}"
|
||||||
|
iyz="0"
|
||||||
|
izz="${(2*r*r*m)/3}" />
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
<!-- TODO Solid cuboid of width w, height h, depth d, and mass m -->
|
||||||
|
<!-- yes, axis in solid_cuboid_inertia are changed, w relates to X axis, d to Z axis, and h to Y axis -->
|
||||||
|
<!-- Keep this comment -->
|
||||||
|
<xacro:macro
|
||||||
|
name="solid_cuboid_inertia"
|
||||||
|
params="m h d w">
|
||||||
|
<inertia
|
||||||
|
ixx="${(m*(h*h+d*d))/12}"
|
||||||
|
ixy="0"
|
||||||
|
ixz="0"
|
||||||
|
iyy="${(m*(w*w+d*d))/12}"
|
||||||
|
iyz="0"
|
||||||
|
izz="${(m*(w*w+h*h))/12}" />
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
<!-- TODO Solid cylinder of radius r, height h and mass m -->
|
||||||
|
<xacro:macro
|
||||||
|
name="solid_cylinder_inertia"
|
||||||
|
params="m r h">
|
||||||
|
<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}" />
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
@@ -0,0 +1,113 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot
|
||||||
|
name="wheel_macro"
|
||||||
|
xmlns:xacro="http://www.ros.org/wiki/xacro"
|
||||||
|
xmlns:gz="http://gazebosim.org/schema">
|
||||||
|
<!--
|
||||||
|
AGV Pro wheel macro.
|
||||||
|
|
||||||
|
Builds a continuous-rotation wheel (link + joint) from a mesh, mass,
|
||||||
|
inertia tensor and mounting origin.
|
||||||
|
|
||||||
|
Params:
|
||||||
|
prefix base name, e.g. 'left_front'
|
||||||
|
mesh STL filename in agv_pro_description/meshes
|
||||||
|
reflect reflect the model and the physics
|
||||||
|
wheel_radius
|
||||||
|
wheel_height
|
||||||
|
wheel_mass link mass (kg)
|
||||||
|
origin_xyz joint origin relative to base_link
|
||||||
|
-->
|
||||||
|
<xacro:macro
|
||||||
|
name="agv_pro_wheel"
|
||||||
|
params="prefix
|
||||||
|
parent
|
||||||
|
origin_xyz
|
||||||
|
reflect
|
||||||
|
mesh
|
||||||
|
wheel_radius:=0.072
|
||||||
|
wheel_height:=0.04
|
||||||
|
wheel_mass:=0.25
|
||||||
|
gazebo_ignition:=false">
|
||||||
|
|
||||||
|
<xacro:macro name="cylinder_inertia" params="m r h">
|
||||||
|
<inertia
|
||||||
|
ixx="${m*(3*r*r+h*h)/12}"
|
||||||
|
ixy="0"
|
||||||
|
ixz="0"
|
||||||
|
iyy="${m*r*r/2}"
|
||||||
|
iyz="0"
|
||||||
|
izz="${m*(3*r*r+h*h)/12}" />
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
<joint name="${prefix}wheel_joint" type="continuous">
|
||||||
|
<origin xyz="${origin_xyz}" rpy="0 0 0" />
|
||||||
|
<parent link="${parent}" />
|
||||||
|
<child link="${prefix}wheel_link" />
|
||||||
|
<axis xyz="0 1 0" />
|
||||||
|
<limit effort="100000" velocity="100" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
<link name="${prefix}wheel_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://agv_pro_description/meshes/wheels/${mesh}" />
|
||||||
|
</geometry>
|
||||||
|
<material name="darkgrey">
|
||||||
|
<color rgba="0.2 0.2 0.2 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<sphere radius="${wheel_radius}" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
|
||||||
|
<inertial>
|
||||||
|
<mass value="${wheel_mass}" />
|
||||||
|
<origin xyz="0 0 0" />
|
||||||
|
<xacro:cylinder_inertia
|
||||||
|
m="${wheel_mass}"
|
||||||
|
r="${wheel_radius}"
|
||||||
|
h="${wheel_height}" />
|
||||||
|
</inertial>
|
||||||
|
|
||||||
|
</link>
|
||||||
|
|
||||||
|
|
||||||
|
<xacro:if value="${gazebo_ignition}">
|
||||||
|
<gazebo reference="${prefix}wheel_link">
|
||||||
|
<collision>
|
||||||
|
<surface>
|
||||||
|
<friction>
|
||||||
|
<ode>
|
||||||
|
<mu>1.0</mu>
|
||||||
|
<mu2>0.0</mu2>
|
||||||
|
<xacro:if value="${reflect}">
|
||||||
|
<fdir1
|
||||||
|
gz:expressed_in="$(arg prefix)base_footprint">1 -1 0</fdir1>
|
||||||
|
</xacro:if>
|
||||||
|
<xacro:unless value="${reflect}">
|
||||||
|
<fdir1
|
||||||
|
gz:expressed_in="$(arg prefix)base_footprint">1 1 0</fdir1>
|
||||||
|
</xacro:unless>
|
||||||
|
</ode>
|
||||||
|
</friction>
|
||||||
|
<contact>
|
||||||
|
<ode>
|
||||||
|
<kp>100000.0</kp>
|
||||||
|
<kd>10.0</kd>
|
||||||
|
</ode>
|
||||||
|
</contact>
|
||||||
|
</surface>
|
||||||
|
</collision>
|
||||||
|
</gazebo>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
@@ -17,23 +17,13 @@ controller_manager:
|
|||||||
# (gz_ros2_control/GazeboSimSystem here vs agv_pro_hardware/AgvProHardwareInterface
|
# (gz_ros2_control/GazeboSimSystem here vs agv_pro_hardware/AgvProHardwareInterface
|
||||||
# on the real robot); the controller parameters are identical.
|
# on the real robot); the controller parameters are identical.
|
||||||
#
|
#
|
||||||
# NOTE: The URDF joint names do not match the physical wheel positions due to a
|
|
||||||
# naming inconsistency in the shared robot model. The controller compensates via
|
|
||||||
# the front/rear/left/right command-joint mapping below:
|
|
||||||
#
|
|
||||||
# Physical position │ URDF joint name
|
|
||||||
# ──────────────────┼────────────────────────────
|
|
||||||
# front-left (FL) │ left_front_wheel_joint ✓
|
|
||||||
# front-right (FR) │ left_rear_wheel_joint ← physically front-right
|
|
||||||
# rear-left (RL) │ right_front_wheel_joint ← physically rear-left
|
|
||||||
# rear-right (RR) │ right_rear_wheel_joint ✓
|
|
||||||
# ─────────────────────────────────────────────────────────────────────────────
|
# ─────────────────────────────────────────────────────────────────────────────
|
||||||
mecanum_drive_controller:
|
mecanum_drive_controller:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
front_left_wheel_command_joint_name: left_front_wheel_joint
|
front_left_wheel_command_joint_name: front_left_wheel_joint
|
||||||
front_right_wheel_command_joint_name: left_rear_wheel_joint
|
front_right_wheel_command_joint_name: front_right_wheel_joint
|
||||||
rear_left_wheel_command_joint_name: right_front_wheel_joint
|
rear_left_wheel_command_joint_name: rear_left_wheel_joint
|
||||||
rear_right_wheel_command_joint_name: right_rear_wheel_joint
|
rear_right_wheel_command_joint_name: rear_right_wheel_joint
|
||||||
|
|
||||||
odom_frame_id: odom
|
odom_frame_id: odom
|
||||||
base_frame_id: base_footprint
|
base_frame_id: base_footprint
|
||||||
|
|||||||
Reference in New Issue
Block a user