Add robot definition URDF
This enables rviz2 to know where sensors etc are. None of the data is correct for the raspbot-v2 yet.
This commit is contained in:
@@ -1,10 +1,22 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.substitutions import LaunchConfiguration, Command
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
urdf_file = os.path.join(
|
||||
get_package_share_directory('raspbot_v2'), 'urdf', 'raspbot_v2.urdf.xacro'
|
||||
)
|
||||
robot_description = ParameterValue(
|
||||
Command(['xacro ', urdf_file]),
|
||||
value_type=str,
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# ── Motor controller arguments ────────────────────────────────────
|
||||
@@ -23,6 +35,33 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('tilt_center_deg', default_value='60.0',
|
||||
description='Tilt angle at startup and shutdown (degrees)'),
|
||||
|
||||
# ── TF / robot description ────────────────────────────────────────
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
parameters=[{'robot_description': robot_description}],
|
||||
output='screen',
|
||||
),
|
||||
|
||||
# Static: map → odom (identity — replace with SLAM/AMCL when ready)
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
name='map_to_odom',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
|
||||
output='screen',
|
||||
),
|
||||
|
||||
# Static: odom → base_footprint (placeholder until odometry is added)
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
name='odom_to_base',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'],
|
||||
output='screen',
|
||||
),
|
||||
|
||||
# ── Nodes ─────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='raspbot_v2',
|
||||
|
||||
@@ -10,6 +10,7 @@ setup(
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name + '/launch', ['launch/robot.launch.py']),
|
||||
('share/' + package_name + '/urdf', ['urdf/raspbot_v2.urdf.xacro']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
|
||||
@@ -0,0 +1,132 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
|
||||
|
||||
<!--
|
||||
Physical dimensions — measure your robot and update these.
|
||||
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 ──────────────────────────────────────────────── -->
|
||||
<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>
|
||||
|
||||
<!-- ── Base link ──────────────────────────────────────────────── -->
|
||||
<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>
|
||||
<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) ────────────────────── -->
|
||||
<link name="base_footprint"/>
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- ── Lidar ──────────────────────────────────────────────────── -->
|
||||
<!--
|
||||
frame_id published by sllidar_ros2 is "laser".
|
||||
Adjust xyz to where the lidar is mounted on your robot.
|
||||
Typical: top-centre, facing forward.
|
||||
-->
|
||||
<link name="laser">
|
||||
<visual>
|
||||
<geometry><cylinder radius="0.04" length="0.04"/></geometry>
|
||||
<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.0 0.0 0.07" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- ── Ultrasonic sensor ──────────────────────────────────────── -->
|
||||
<!--
|
||||
frame_id published by ultrasonic_node is "ultrasonic".
|
||||
Typical: 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>
|
||||
</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 ──────────────────────────────────── -->
|
||||
<!--
|
||||
The camera_orientation node publishes joint_states for joints
|
||||
named "pan" and "tilt". robot_state_publisher will drive these
|
||||
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) -->
|
||||
<link name="camera_pan_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.015" rpy="0 0 0"/>
|
||||
<limit lower="-1.5708" upper="1.5708" effort="1.0" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<!-- Tilt joint (pitch, around Y) -->
|
||||
<link name="camera_link">
|
||||
<visual>
|
||||
<geometry><box size="0.03 0.06 0.02"/></geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="tilt" type="revolute">
|
||||
<parent link="camera_pan_link"/>
|
||||
<child link="camera_link"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin xyz="0.02 0 0" rpy="0 0 0"/>
|
||||
<limit lower="-0.5236" upper="0.9599" effort="1.0" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
Reference in New Issue
Block a user