Refactor the URDF into multiple parts.
Makes it easier to work on an individual part in isolation. Also change the vscode devconsole visualiser to one that works with includes
This commit is contained in:
@@ -46,6 +46,11 @@ RUN apt-get update && apt-get install -y \
|
|||||||
# ros-${ROS_DISTRO}-ament-lint-auto \
|
# ros-${ROS_DISTRO}-ament-lint-auto \
|
||||||
# ros-${ROS_DISTRO}-ament-lint-common
|
# ros-${ROS_DISTRO}-ament-lint-common
|
||||||
|
|
||||||
|
# URDF / xacro tooling — needed to generate flat URDF for IDE preview
|
||||||
|
RUN apt-get update && apt-get install -y \
|
||||||
|
ros-${ROS_DISTRO}-xacro \
|
||||||
|
ros-${ROS_DISTRO}-robot-state-publisher
|
||||||
|
|
||||||
# Node.js for frontend development
|
# Node.js for frontend development
|
||||||
RUN curl -fsSL https://deb.nodesource.com/setup_22.x | bash - \
|
RUN curl -fsSL https://deb.nodesource.com/setup_22.x | bash - \
|
||||||
&& apt-get install -y nodejs
|
&& apt-get install -y nodejs
|
||||||
|
|||||||
@@ -21,7 +21,7 @@
|
|||||||
"eamodio.gitlens",
|
"eamodio.gitlens",
|
||||||
"ms-iot.vscode-ros",
|
"ms-iot.vscode-ros",
|
||||||
"anthropic.claude-code",
|
"anthropic.claude-code",
|
||||||
"Ranch-Hand-Robotics.urdf-editor"
|
"morningfrog.urdf-visualizer"
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
|||||||
@@ -4,6 +4,9 @@
|
|||||||
.vscode/browse.vc.db-wal
|
.vscode/browse.vc.db-wal
|
||||||
.vscode/c_cpp_properties.json
|
.vscode/c_cpp_properties.json
|
||||||
|
|
||||||
|
# Generated URDF preview (regenerate with the "Generate preview URDF" VS Code task)
|
||||||
|
**/urdf/preview.urdf
|
||||||
|
|
||||||
# colcon build artefacts
|
# colcon build artefacts
|
||||||
build/
|
build/
|
||||||
install/
|
install/
|
||||||
|
|||||||
@@ -13,7 +13,10 @@ def generate_launch_description():
|
|||||||
get_package_share_directory('raspbot_v2'), 'urdf', 'raspbot_v2.urdf.xacro'
|
get_package_share_directory('raspbot_v2'), 'urdf', 'raspbot_v2.urdf.xacro'
|
||||||
)
|
)
|
||||||
robot_description = ParameterValue(
|
robot_description = ParameterValue(
|
||||||
Command(['xacro ', urdf_file]),
|
Command([
|
||||||
|
'xacro ', urdf_file,
|
||||||
|
' wheel_separation:=', LaunchConfiguration('wheel_base'),
|
||||||
|
]),
|
||||||
value_type=str,
|
value_type=str,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@@ -1,3 +1,5 @@
|
|||||||
|
import glob
|
||||||
|
|
||||||
from setuptools import setup
|
from setuptools import setup
|
||||||
|
|
||||||
package_name = 'raspbot_v2'
|
package_name = 'raspbot_v2'
|
||||||
@@ -10,7 +12,7 @@ setup(
|
|||||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||||
('share/' + package_name, ['package.xml']),
|
('share/' + package_name, ['package.xml']),
|
||||||
('share/' + package_name + '/launch', ['launch/robot.launch.py']),
|
('share/' + package_name + '/launch', ['launch/robot.launch.py']),
|
||||||
('share/' + package_name + '/urdf', ['urdf/raspbot_v2.urdf.xacro']),
|
('share/' + package_name + '/urdf', glob.glob('urdf/*.xacro')),
|
||||||
],
|
],
|
||||||
install_requires=['setuptools'],
|
install_requires=['setuptools'],
|
||||||
zip_safe=True,
|
zip_safe=True,
|
||||||
|
|||||||
@@ -0,0 +1,83 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Camera module
|
||||||
|
|
||||||
|
Mounted on camera_tilt_link (output of the tilt joint in pan_tilt.xacro).
|
||||||
|
|
||||||
|
Two frames are defined:
|
||||||
|
camera_link — physical body of the camera module
|
||||||
|
camera_optical_frame — REP-103 optical frame (Z forward, X right, Y down)
|
||||||
|
used as the sensor origin in Gazebo and ROS image topics
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<!-- Physical camera body — visual only, small box representing the module -->
|
||||||
|
<link name="camera_link">
|
||||||
|
<visual>
|
||||||
|
<geometry><box size="0.025 0.06 0.02"/></geometry>
|
||||||
|
<material name="blue"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry><box size="0.025 0.06 0.02"/></geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.03"/>
|
||||||
|
<inertia ixx="0.000005" ixy="0" ixz="0"
|
||||||
|
iyy="0.000005" iyz="0" izz="0.000005"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="camera_joint" type="fixed">
|
||||||
|
<parent link="camera_tilt_link"/>
|
||||||
|
<child link="camera_link"/>
|
||||||
|
<origin xyz="0.01 0 0" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- REP-103 optical frame: Z forward, X right, Y down.
|
||||||
|
The rpy="-pi/2 0 -pi/2" rotation maps the body frame (X-fwd, Y-left, Z-up)
|
||||||
|
to the optical frame convention expected by image_geometry and cv_bridge. -->
|
||||||
|
<link name="camera_optical_frame"/>
|
||||||
|
|
||||||
|
<joint name="camera_optical_joint" type="fixed">
|
||||||
|
<parent link="camera_link"/>
|
||||||
|
<child link="camera_optical_frame"/>
|
||||||
|
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Gazebo camera sensor (Gazebo Harmonic / gz-sim)
|
||||||
|
|
||||||
|
Image data is published on gz topics and bridged to ROS 2 via
|
||||||
|
ros_gz_bridge:
|
||||||
|
/camera/image (gz) → /camera/image_raw (ROS 2)
|
||||||
|
/camera/info (gz) → /camera/camera_info (ROS 2)
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
<gazebo reference="camera_link">
|
||||||
|
<sensor name="camera" type="camera">
|
||||||
|
<always_on>1</always_on>
|
||||||
|
<update_rate>30</update_rate>
|
||||||
|
<visualize>true</visualize>
|
||||||
|
<camera>
|
||||||
|
<horizontal_fov>${camera_fov}</horizontal_fov>
|
||||||
|
<image>
|
||||||
|
<width>${camera_width}</width>
|
||||||
|
<height>${camera_height}</height>
|
||||||
|
<format>R8G8B8</format>
|
||||||
|
</image>
|
||||||
|
<clip>
|
||||||
|
<near>0.05</near>
|
||||||
|
<far>50.0</far>
|
||||||
|
</clip>
|
||||||
|
<noise>
|
||||||
|
<type>gaussian</type>
|
||||||
|
<mean>0.0</mean>
|
||||||
|
<stddev>0.007</stddev>
|
||||||
|
</noise>
|
||||||
|
</camera>
|
||||||
|
<!-- Publishes to gz topic /camera/image -->
|
||||||
|
<topic>/camera/image</topic>
|
||||||
|
</sensor>
|
||||||
|
</gazebo>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@@ -0,0 +1,77 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
RPLIDAR A1 — 2D laser scanner
|
||||||
|
|
||||||
|
frame_id "laser" matches what sllidar_ros2 publishes on /scan.
|
||||||
|
Adjust xyz to the physical mounting position on the chassis.
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<link name="laser">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.04" length="0.04"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="light_grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.04" length="0.04"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.17"/>
|
||||||
|
<!-- Solid cylinder -->
|
||||||
|
<inertia ixx="0.000087" ixy="0" ixz="0"
|
||||||
|
iyy="0.000087" iyz="0"
|
||||||
|
izz="0.000136"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="laser_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="laser"/>
|
||||||
|
<!-- x/y/z: position of lidar centre relative to base_link centre.
|
||||||
|
x negative = rear-mounted; adjust to your physical layout. -->
|
||||||
|
<origin xyz="${lidar_x} ${lidar_y} ${lidar_z}" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Gazebo gpu_lidar sensor (Gazebo Harmonic / gz-sim)
|
||||||
|
|
||||||
|
Scan data is published on gz topic /scan and bridged to ROS 2 via
|
||||||
|
ros_gz_bridge:
|
||||||
|
/scan (gz) → /scan (ROS 2, sensor_msgs/LaserScan)
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
<gazebo reference="laser">
|
||||||
|
<sensor name="lidar" type="gpu_lidar">
|
||||||
|
<always_on>1</always_on>
|
||||||
|
<update_rate>10</update_rate>
|
||||||
|
<visualize>true</visualize>
|
||||||
|
<topic>/scan</topic>
|
||||||
|
<gz_frame_id>laser</gz_frame_id>
|
||||||
|
<lidar>
|
||||||
|
<scan>
|
||||||
|
<horizontal>
|
||||||
|
<samples>360</samples>
|
||||||
|
<resolution>1</resolution>
|
||||||
|
<min_angle>-3.14159265</min_angle>
|
||||||
|
<max_angle> 3.14159265</max_angle>
|
||||||
|
</horizontal>
|
||||||
|
</scan>
|
||||||
|
<range>
|
||||||
|
<min>${lidar_min_range}</min>
|
||||||
|
<max>${lidar_max_range}</max>
|
||||||
|
<resolution>0.01</resolution>
|
||||||
|
</range>
|
||||||
|
<noise>
|
||||||
|
<type>gaussian</type>
|
||||||
|
<mean>0.0</mean>
|
||||||
|
<stddev>0.01</stddev>
|
||||||
|
</noise>
|
||||||
|
</lidar>
|
||||||
|
</sensor>
|
||||||
|
</gazebo>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@@ -0,0 +1,134 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Camera pan / tilt mechanism
|
||||||
|
|
||||||
|
Joint names (pan, tilt) must match the names published by
|
||||||
|
camera_orientation_node on /joint_states so that robot_state_publisher
|
||||||
|
drives the TF tree correctly from live hardware.
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<!-- Mount bracket — fixed to the top-front of the chassis -->
|
||||||
|
<link name="camera_base">
|
||||||
|
<visual>
|
||||||
|
<geometry><box size="0.03 0.04 0.03"/></geometry>
|
||||||
|
<material name="dark_grey"/>
|
||||||
|
</visual>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.05"/>
|
||||||
|
<inertia ixx="0.00001" ixy="0" ixz="0"
|
||||||
|
iyy="0.00001" iyz="0" izz="0.00001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="camera_base_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="camera_base"/>
|
||||||
|
<!-- Adjust xyz to match the physical servo mount position -->
|
||||||
|
<origin xyz="${base_length/2 - 0.02} 0.0 ${base_height/2 + 0.015}" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- Pan link — rotates around Z (yaw) -->
|
||||||
|
<link name="camera_pan_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01"/>
|
||||||
|
<inertia ixx="0.000001" ixy="0" ixz="0"
|
||||||
|
iyy="0.000001" iyz="0" izz="0.000001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="pan" type="revolute">
|
||||||
|
<parent link="camera_base"/>
|
||||||
|
<child link="camera_pan_link"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<origin xyz="0 0 0.02" rpy="0 0 0"/>
|
||||||
|
<!-- Physical range: 0°–180°, centred at 90° → ROS range: ±90° -->
|
||||||
|
<limit lower="${pan_min_rad}" upper="${pan_max_rad}"
|
||||||
|
effort="1.0" velocity="1.5"/>
|
||||||
|
<dynamics damping="0.1" friction="0.05"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- Tilt link — rotates around Y (pitch) -->
|
||||||
|
<link name="camera_tilt_link">
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.02"/>
|
||||||
|
<inertia ixx="0.000002" ixy="0" ixz="0"
|
||||||
|
iyy="0.000002" iyz="0" izz="0.000002"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="tilt" type="revolute">
|
||||||
|
<parent link="camera_pan_link"/>
|
||||||
|
<child link="camera_tilt_link"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
<origin xyz="0.02 0 0" rpy="0 0 0"/>
|
||||||
|
<!-- Physical range: 0°–110°, centred at 60° → ROS range: -60° to +50° -->
|
||||||
|
<limit lower="${tilt_min_rad}" upper="${tilt_max_rad}"
|
||||||
|
effort="1.0" velocity="1.5"/>
|
||||||
|
<dynamics damping="0.1" friction="0.05"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Transmissions (for ros2_control)
|
||||||
|
|
||||||
|
These describe the mapping between joint commands and actuators.
|
||||||
|
Required when using gz_ros2_control for Gazebo simulation.
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<transmission name="pan_transmission">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="pan">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="pan_actuator">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
|
||||||
|
<transmission name="tilt_transmission">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="tilt">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="tilt_actuator">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>1</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
ros2_control block
|
||||||
|
|
||||||
|
For Gazebo simulation, set the hardware plugin to:
|
||||||
|
gz_ros2_control/GazeboSimSystem
|
||||||
|
For the real robot, replace with the custom I²C servo hardware plugin.
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
<ros2_control name="pan_tilt_servos" type="system">
|
||||||
|
<hardware>
|
||||||
|
<!-- Gazebo simulation: -->
|
||||||
|
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||||
|
<!-- Real robot (replace with actual plugin when available):
|
||||||
|
<plugin>raspbot_v2_hardware/ServoHardware</plugin>
|
||||||
|
-->
|
||||||
|
</hardware>
|
||||||
|
<joint name="pan">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">${pan_min_rad}</param>
|
||||||
|
<param name="max">${pan_max_rad}</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="tilt">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">${tilt_min_rad}</param>
|
||||||
|
<param name="max">${tilt_max_rad}</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@@ -1,132 +1,99 @@
|
|||||||
<?xml version="1.0"?>
|
<?xml version="1.0"?>
|
||||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
|
||||||
|
|
||||||
<!--
|
<!-- ═══════════════════════════════════════════════════════════════════════
|
||||||
Physical dimensions — measure your robot and update these.
|
raspbot_v2.urdf.xacro — top-level composition file
|
||||||
All values in metres.
|
|
||||||
-->
|
|
||||||
<xacro:property name="base_length" value="0.20"/>
|
|
||||||
<xacro:property name="base_width" value="0.15"/>
|
|
||||||
<xacro:property name="base_height" value="0.06"/>
|
|
||||||
|
|
||||||
<!-- ── Materials ──────────────────────────────────────────────── -->
|
All physical parameters are declared here as xacro:arg so they can
|
||||||
<material name="dark_grey">
|
be overridden at build time:
|
||||||
<color rgba="0.2 0.2 0.2 1.0"/>
|
xacro raspbot_v2.urdf.xacro wheel_separation:=0.14
|
||||||
</material>
|
|
||||||
<material name="light_grey">
|
|
||||||
<color rgba="0.7 0.7 0.7 1.0"/>
|
|
||||||
</material>
|
|
||||||
<material name="blue">
|
|
||||||
<color rgba="0.0 0.4 0.8 1.0"/>
|
|
||||||
</material>
|
|
||||||
|
|
||||||
<!-- ── Base link ──────────────────────────────────────────────── -->
|
Sub-files reference these properties directly; they are not intended
|
||||||
<link name="base_link">
|
to be included in isolation.
|
||||||
<visual>
|
═══════════════════════════════════════════════════════════════════════ -->
|
||||||
<geometry>
|
|
||||||
<box size="${base_length} ${base_width} ${base_height}"/>
|
|
||||||
</geometry>
|
|
||||||
<material name="dark_grey"/>
|
|
||||||
</visual>
|
|
||||||
<collision>
|
|
||||||
<geometry>
|
|
||||||
<box size="${base_length} ${base_width} ${base_height}"/>
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
<inertial>
|
|
||||||
<mass value="1.0"/>
|
|
||||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
|
|
||||||
iyy="0.01" iyz="0.0" izz="0.02"/>
|
|
||||||
</inertial>
|
|
||||||
</link>
|
|
||||||
|
|
||||||
<!-- ── Footprint (ground-projected origin) ────────────────────── -->
|
<!-- ── Chassis ────────────────────────────────────────────────────────── -->
|
||||||
<link name="base_footprint"/>
|
<xacro:arg name="base_length" default="0.20"/> <!-- m, front-to-rear -->
|
||||||
<joint name="base_footprint_joint" type="fixed">
|
<xacro:arg name="base_width" default="0.15"/> <!-- m, left-to-right -->
|
||||||
<parent link="base_footprint"/>
|
<xacro:arg name="base_height" default="0.06"/> <!-- m -->
|
||||||
<child link="base_link"/>
|
<xacro:arg name="base_mass" default="1.0"/> <!-- kg -->
|
||||||
<origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<!-- ── Lidar ──────────────────────────────────────────────────── -->
|
<!-- ── Wheels ─────────────────────────────────────────────────────────── -->
|
||||||
<!--
|
<!-- wheel_separation: centre-to-centre distance between left and right wheels.
|
||||||
frame_id published by sllidar_ros2 is "laser".
|
Should match the wheel_base parameter in the motor controller node. -->
|
||||||
Adjust xyz to where the lidar is mounted on your robot.
|
<xacro:arg name="wheel_separation" default="0.14"/> <!-- m -->
|
||||||
Typical: top-centre, facing forward.
|
<xacro:arg name="wheel_radius" default="0.033"/> <!-- m -->
|
||||||
-->
|
<xacro:arg name="wheel_width" default="0.02"/> <!-- m -->
|
||||||
<link name="laser">
|
<xacro:arg name="wheel_mass" default="0.1"/> <!-- kg -->
|
||||||
<visual>
|
<!-- Front-to-rear distance between wheel pairs (half-value used per side) -->
|
||||||
<geometry><cylinder radius="0.04" length="0.04"/></geometry>
|
<xacro:arg name="wheel_base_half" default="0.07"/> <!-- m -->
|
||||||
<material name="light_grey"/>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
<joint name="laser_joint" type="fixed">
|
|
||||||
<parent link="base_link"/>
|
|
||||||
<child link="laser"/>
|
|
||||||
<!-- Adjust x/y/z: x=forward, y=left, z=up from base_link centre -->
|
|
||||||
<origin xyz="-0.03 0.0 0.09" rpy="0 0 0"/>
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<!-- ── Ultrasonic sensor ──────────────────────────────────────── -->
|
<!-- ── Pan / tilt limits (radians, ROS convention: 0 = centre) ────────── -->
|
||||||
<!--
|
<!-- Pan: physical 0°–180°, centred at 90° → ±π/2 -->
|
||||||
frame_id published by ultrasonic_node is "ultrasonic".
|
<!-- Tilt: physical 0°–110°, centred at 60° → −60° (−1.047) to +50° (+0.873) -->
|
||||||
Typical: front face of the chassis, pointing forward.
|
<xacro:arg name="pan_min_rad" default="${-pi/2}"/>
|
||||||
-->
|
<xacro:arg name="pan_max_rad" default="${ pi/2}"/>
|
||||||
<link name="ultrasonic">
|
<xacro:arg name="tilt_min_rad" default="${-pi/3}"/> <!-- −60° -->
|
||||||
<visual>
|
<xacro:arg name="tilt_max_rad" default="${5*pi/18}"/> <!-- +50° -->
|
||||||
<geometry><box size="0.02 0.04 0.02"/></geometry>
|
|
||||||
<material name="light_grey"/>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
<joint name="ultrasonic_joint" type="fixed">
|
|
||||||
<parent link="base_link"/>
|
|
||||||
<child link="ultrasonic"/>
|
|
||||||
<!-- Front-centre of the chassis, roughly mid-height -->
|
|
||||||
<origin xyz="${base_length/2} 0.0 0.0" rpy="0 0 0"/>
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<!-- ── Camera pan/tilt mount ──────────────────────────────────── -->
|
<!-- ── Camera ─────────────────────────────────────────────────────────── -->
|
||||||
<!--
|
<xacro:arg name="camera_fov" default="1.047"/> <!-- rad (~60°) -->
|
||||||
The camera_orientation node publishes joint_states for joints
|
<xacro:arg name="camera_width" default="640"/>
|
||||||
named "pan" and "tilt". robot_state_publisher will drive these
|
<xacro:arg name="camera_height" default="480"/>
|
||||||
dynamically from that topic.
|
|
||||||
-->
|
|
||||||
<link name="camera_base">
|
|
||||||
<visual>
|
|
||||||
<geometry><box size="0.03 0.03 0.03"/></geometry>
|
|
||||||
<material name="dark_grey"/>
|
|
||||||
</visual>
|
|
||||||
</link>
|
|
||||||
<joint name="camera_base_joint" type="fixed">
|
|
||||||
<parent link="base_link"/>
|
|
||||||
<child link="camera_base"/>
|
|
||||||
<!-- Top-front of robot; adjust to match servo mount position -->
|
|
||||||
<origin xyz="0.05 0.0 0.06" rpy="0 0 0"/>
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<!-- Pan joint (yaw, around Z) -->
|
<!-- ── Lidar ──────────────────────────────────────────────────────────── -->
|
||||||
<link name="camera_pan_link"/>
|
<!-- Mounting position relative to base_link centre -->
|
||||||
<joint name="pan" type="revolute">
|
<xacro:arg name="lidar_x" default="-0.03"/> <!-- m, slightly rearward -->
|
||||||
<parent link="camera_base"/>
|
<xacro:arg name="lidar_y" default="0.0"/>
|
||||||
<child link="camera_pan_link"/>
|
<xacro:arg name="lidar_z" default="0.09"/> <!-- m above base_link centre -->
|
||||||
<axis xyz="0 0 1"/>
|
<xacro:arg name="lidar_min_range" default="0.15"/> <!-- m (RPLIDAR A1 spec) -->
|
||||||
<origin xyz="0 0 0.015" rpy="0 0 0"/>
|
<xacro:arg name="lidar_max_range" default="12.0"/> <!-- m (RPLIDAR A1 spec) -->
|
||||||
<limit lower="-1.5708" upper="1.5708" effort="1.0" velocity="1.0"/>
|
|
||||||
</joint>
|
|
||||||
|
|
||||||
<!-- Tilt joint (pitch, around Y) -->
|
<!-- ── Sonar ──────────────────────────────────────────────────────────── -->
|
||||||
<link name="camera_link">
|
<xacro:arg name="sonar_min_range" default="0.02"/> <!-- m (HC-SR04 spec) -->
|
||||||
<visual>
|
<xacro:arg name="sonar_max_range" default="4.0"/> <!-- m (HC-SR04 spec) -->
|
||||||
<geometry><box size="0.03 0.06 0.02"/></geometry>
|
|
||||||
<material name="blue"/>
|
<!-- ═══════════════════════════════════════════════════════════════════════
|
||||||
</visual>
|
Resolve args → properties so sub-files can use ${name} syntax
|
||||||
</link>
|
═══════════════════════════════════════════════════════════════════════ -->
|
||||||
<joint name="tilt" type="revolute">
|
|
||||||
<parent link="camera_pan_link"/>
|
<xacro:property name="base_length" value="$(arg base_length)"/>
|
||||||
<child link="camera_link"/>
|
<xacro:property name="base_width" value="$(arg base_width)"/>
|
||||||
<axis xyz="0 1 0"/>
|
<xacro:property name="base_height" value="$(arg base_height)"/>
|
||||||
<origin xyz="0.02 0 0" rpy="0 0 0"/>
|
<xacro:property name="base_mass" value="$(arg base_mass)"/>
|
||||||
<limit lower="-1.0472" upper="0.7854" effort="1.0" velocity="1.0"/>
|
|
||||||
</joint>
|
<xacro:property name="wheel_separation" value="$(arg wheel_separation)"/>
|
||||||
|
<xacro:property name="wheel_radius" value="$(arg wheel_radius)"/>
|
||||||
|
<xacro:property name="wheel_width" value="$(arg wheel_width)"/>
|
||||||
|
<xacro:property name="wheel_mass" value="$(arg wheel_mass)"/>
|
||||||
|
<xacro:property name="wheel_base_half" value="$(arg wheel_base_half)"/>
|
||||||
|
|
||||||
|
<xacro:property name="pan_min_rad" value="$(arg pan_min_rad)"/>
|
||||||
|
<xacro:property name="pan_max_rad" value="$(arg pan_max_rad)"/>
|
||||||
|
<xacro:property name="tilt_min_rad" value="$(arg tilt_min_rad)"/>
|
||||||
|
<xacro:property name="tilt_max_rad" value="$(arg tilt_max_rad)"/>
|
||||||
|
|
||||||
|
<xacro:property name="camera_fov" value="$(arg camera_fov)"/>
|
||||||
|
<xacro:property name="camera_width" value="$(arg camera_width)"/>
|
||||||
|
<xacro:property name="camera_height" value="$(arg camera_height)"/>
|
||||||
|
|
||||||
|
<xacro:property name="lidar_x" value="$(arg lidar_x)"/>
|
||||||
|
<xacro:property name="lidar_y" value="$(arg lidar_y)"/>
|
||||||
|
<xacro:property name="lidar_z" value="$(arg lidar_z)"/>
|
||||||
|
<xacro:property name="lidar_min_range" value="$(arg lidar_min_range)"/>
|
||||||
|
<xacro:property name="lidar_max_range" value="$(arg lidar_max_range)"/>
|
||||||
|
|
||||||
|
<xacro:property name="sonar_min_range" value="$(arg sonar_min_range)"/>
|
||||||
|
<xacro:property name="sonar_max_range" value="$(arg sonar_max_range)"/>
|
||||||
|
|
||||||
|
<!-- ═══════════════════════════════════════════════════════════════════════
|
||||||
|
Sub-file includes (order matters — base must come first)
|
||||||
|
═══════════════════════════════════════════════════════════════════════ -->
|
||||||
|
|
||||||
|
<xacro:include filename="robot_base.xacro"/>
|
||||||
|
<xacro:include filename="pan_tilt.xacro"/>
|
||||||
|
<xacro:include filename="camera.xacro"/>
|
||||||
|
<xacro:include filename="lidar.xacro"/>
|
||||||
|
<xacro:include filename="sonar.xacro"/>
|
||||||
|
|
||||||
</robot>
|
</robot>
|
||||||
|
|||||||
@@ -0,0 +1,133 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Inertia helpers
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<xacro:macro name="box_inertia" params="m x y z">
|
||||||
|
<inertial>
|
||||||
|
<mass value="${m}"/>
|
||||||
|
<inertia ixx="${m*(y*y+z*z)/12}" ixy="0" ixz="0"
|
||||||
|
iyy="${m*(x*x+z*z)/12}" iyz="0"
|
||||||
|
izz="${m*(x*x+y*y)/12}"/>
|
||||||
|
</inertial>
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
<!-- Cylinder axis along Z -->
|
||||||
|
<xacro:macro name="cylinder_inertia" 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>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Materials
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<material name="dark_grey"> <color rgba="0.2 0.2 0.2 1.0"/></material>
|
||||||
|
<material name="light_grey"> <color rgba="0.7 0.7 0.7 1.0"/></material>
|
||||||
|
<material name="blue"> <color rgba="0.0 0.4 0.8 1.0"/></material>
|
||||||
|
<material name="black"> <color rgba="0.05 0.05 0.05 1.0"/></material>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Base chassis
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<link name="base_footprint"/>
|
||||||
|
|
||||||
|
<link name="base_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<box size="${base_length} ${base_width} ${base_height}"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="dark_grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="${base_length} ${base_width} ${base_height}"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<xacro:box_inertia m="${base_mass}"
|
||||||
|
x="${base_length}" y="${base_width}" z="${base_height}"/>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<!-- base_footprint is the ground-projected origin; base_link is the chassis body -->
|
||||||
|
<joint name="base_footprint_joint" type="fixed">
|
||||||
|
<parent link="base_footprint"/>
|
||||||
|
<child link="base_link"/>
|
||||||
|
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Wheels (FL / FR / RL / RR)
|
||||||
|
Cylinders rotated 90° so their axis lies along Y (roll axis).
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<xacro:macro name="wheel" params="name x_sign y_sign">
|
||||||
|
<link name="wheel_${name}">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
|
||||||
|
<material name="black"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
|
||||||
|
</geometry>
|
||||||
|
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
|
||||||
|
</collision>
|
||||||
|
<xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="wheel_${name}_joint" type="continuous">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="wheel_${name}"/>
|
||||||
|
<!-- x_sign: +1=front, -1=rear | y_sign: +1=left, -1=right -->
|
||||||
|
<origin xyz="${x_sign * wheel_base_half} ${y_sign * wheel_separation/2} ${-base_height/2 + wheel_radius}"
|
||||||
|
rpy="0 0 0"/>
|
||||||
|
<axis xyz="0 1 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- Friction and contact properties for Gazebo -->
|
||||||
|
<gazebo reference="wheel_${name}">
|
||||||
|
<mu1>1.0</mu1>
|
||||||
|
<mu2>0.5</mu2>
|
||||||
|
<kp>1e6</kp>
|
||||||
|
<kd>1.0</kd>
|
||||||
|
</gazebo>
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
<xacro:wheel name="front_left" x_sign=" 1" y_sign=" 1"/>
|
||||||
|
<xacro:wheel name="front_right" x_sign=" 1" y_sign="-1"/>
|
||||||
|
<xacro:wheel name="rear_left" x_sign="-1" y_sign=" 1"/>
|
||||||
|
<xacro:wheel name="rear_right" x_sign="-1" y_sign="-1"/>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Gazebo — differential drive plugin
|
||||||
|
Bridges to /cmd_vel and publishes /odom.
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
<gazebo>
|
||||||
|
<plugin filename="gz-sim-diff-drive-system"
|
||||||
|
name="gz::sim::systems::DiffDrive">
|
||||||
|
<!-- Use averaged front/rear pairs for each side -->
|
||||||
|
<left_joint>wheel_front_left_joint</left_joint>
|
||||||
|
<left_joint>wheel_rear_left_joint</left_joint>
|
||||||
|
<right_joint>wheel_front_right_joint</right_joint>
|
||||||
|
<right_joint>wheel_rear_right_joint</right_joint>
|
||||||
|
<wheel_separation>${wheel_separation}</wheel_separation>
|
||||||
|
<wheel_radius>${wheel_radius}</wheel_radius>
|
||||||
|
<odom_publish_frequency>10</odom_publish_frequency>
|
||||||
|
<topic>cmd_vel</topic>
|
||||||
|
<odom_topic>odom</odom_topic>
|
||||||
|
<frame_id>odom</frame_id>
|
||||||
|
<child_frame_id>base_footprint</child_frame_id>
|
||||||
|
</plugin>
|
||||||
|
</gazebo>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@@ -0,0 +1,58 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
HC-SR04 ultrasonic range sensor
|
||||||
|
|
||||||
|
frame_id "ultrasonic" matches what ultrasonic_node publishes on
|
||||||
|
/ultrasonic/range. Mounted on the front face of the chassis,
|
||||||
|
pointing forward.
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
|
||||||
|
<link name="ultrasonic">
|
||||||
|
<visual>
|
||||||
|
<geometry><box size="0.02 0.04 0.02"/></geometry>
|
||||||
|
<material name="light_grey"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry><box size="0.02 0.04 0.02"/></geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<mass value="0.01"/>
|
||||||
|
<inertia ixx="0.000001" ixy="0" ixz="0"
|
||||||
|
iyy="0.000001" iyz="0" izz="0.000001"/>
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="ultrasonic_joint" type="fixed">
|
||||||
|
<parent link="base_link"/>
|
||||||
|
<child link="ultrasonic"/>
|
||||||
|
<!-- Front-centre of the chassis, mid-height -->
|
||||||
|
<origin xyz="${base_length/2 + 0.01} 0.0 0.0" rpy="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<!-- ─────────────────────────────────────────────────────────────────────
|
||||||
|
Gazebo sonar sensor (Gazebo Harmonic / gz-sim)
|
||||||
|
|
||||||
|
Simulated as a narrow-cone ray sensor. Data is published on gz topic
|
||||||
|
/ultrasonic/range and bridged to ROS 2 via ros_gz_bridge:
|
||||||
|
/ultrasonic/range (gz) → /ultrasonic/range (ROS 2, sensor_msgs/Range)
|
||||||
|
───────────────────────────────────────────────────────────────────── -->
|
||||||
|
<gazebo reference="ultrasonic">
|
||||||
|
<sensor name="sonar" type="sonar">
|
||||||
|
<always_on>1</always_on>
|
||||||
|
<update_rate>10</update_rate>
|
||||||
|
<visualize>true</visualize>
|
||||||
|
<topic>/ultrasonic/range</topic>
|
||||||
|
<gz_frame_id>ultrasonic</gz_frame_id>
|
||||||
|
<sonar>
|
||||||
|
<!-- HC-SR04 datasheet values -->
|
||||||
|
<min>${sonar_min_range}</min>
|
||||||
|
<max>${sonar_max_range}</max>
|
||||||
|
<!-- Half-angle of the cone in radians (~15° full FOV → 0.13 rad half-angle) -->
|
||||||
|
<radius>0.13</radius>
|
||||||
|
</sonar>
|
||||||
|
</sensor>
|
||||||
|
</gazebo>
|
||||||
|
|
||||||
|
</robot>
|
||||||
Reference in New Issue
Block a user