Update model

README.md

@@ -53,11 +53,23 @@ Supported parameters:
 Displays the robot's model in rviz, starts joint_state_publisher and robot_state_publisher
 ```bash
 #### Load generation 3 model
-ros2 launch robotont_description display_simulated_robot.launch.py
+ros2 launch robotont_description display_robot_model.launch.py
 ```
+Displays the robot's model in rviz, with different frame colors (blue, light_blue, purple, yellow, green, dark_green, black, gray):
+```bash
+#### Load generation 3 model with specific frame color
+ros2 launch robotont_description display_robot_model.launch.py primary_color:=light_blue
+```
+
+Supported formats for `primary_color`:
+- color name:      primary_color:=lightblue 
+- RGBA (0..1):     primary_color:="0.16 0.65 0.98 1.0"
+- RGBA (0..255):   primary_color:="41 166 250 255"
+- HEX:             primary_color:="#29a6faff" 
+
 ```bash
 #### Load generation 2.1 model
-ros2 launch robotont_description display_simulated_robot.launch.py generation:=2.1
+ros2 launch robotont_description display_robot_model.launch.py generation:=2.1
 ```
 #### 2.2. Description
 Starts joint_state_publisher and robot_state_publisher, robot model is published on /robot_description topic.

launch/display_robot_model.launch.py

@@ -0,0 +1,123 @@
+from ament_index_python.packages import get_package_share_path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, Command
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def _rgba_string_from_user_value(user_value: str) -> str:
+    s = (user_value or "").strip()
+    if not s:
+        raise ValueError("Empty color string")
+
+    # RGBA numeric input (space or comma separated)
+    parts = s.replace(",", " ").split()
+    if len(parts) == 4:
+        nums = [float(p) for p in parts]
+        if any(v > 1.0 for v in nums):
+            nums = [v / 255.0 for v in nums]
+        nums = [min(1.0, max(0.0, v)) for v in nums]
+        return f"{nums[0]} {nums[1]} {nums[2]} {nums[3]}"
+
+    # Hex input
+    if s.startswith("#") and len(s) in (7, 9):
+        hexv = s[1:]
+        r = int(hexv[0:2], 16)
+        g = int(hexv[2:4], 16)
+        b = int(hexv[4:6], 16)
+        a = int(hexv[6:8], 16) if len(hexv) == 8 else 255
+        return f"{r/255.0} {g/255.0} {b/255.0} {a/255.0}"
+
+    # Color names
+    name = s.lower().replace("_", "").replace("-", "")
+    try:
+        from PIL import ImageColor
+        r, g, b, a = ImageColor.getcolor(name, "RGBA")
+        return f"{r/255.0} {g/255.0} {b/255.0} {a/255.0}"
+    except Exception:
+        fallback = {
+            "lightblue": "0.16 0.65 0.98 1.0",
+            "blue":      "0.00 0.35 0.90 1.0",
+            "yellow":    "1.00 1.00 0.00 1.0",
+            "black":     "0.10 0.10 0.10 1.0",
+            "purple":    "0.45 0.20 0.65 1.0",
+            "gray":      "0.75 0.75 0.75 1.0",
+            "darkgreen": "0.00 0.45 0.25 1.0",
+            "green":     "0.00 0.80 0.30 1.0",
+        }
+        if name in fallback:
+            return fallback[name]
+        raise ValueError(
+            f"Unknown color '{user_value}'. Provide RGBA ('0 1 0 1' or '0 255 0 255'), "
+            f"hex ('#00ff00' or '#00ff00ff'), or install Pillow for CSS color names."
+        )
+
+
+def _setup(context, *args, **kwargs):
+    pkg = get_package_share_path("robotont_description")
+
+    model = LaunchConfiguration("model").perform(context).strip()
+    generation = LaunchConfiguration("generation").perform(context).strip()
+
+    primary_in = LaunchConfiguration("primary_color").perform(context)
+
+    if model:
+        robot_model_path = model
+    else:
+        if generation == "2.1":
+            robot_model_path = str(pkg / "urdf/gen2_1/robotont.urdf.xacro")
+        else:
+            # Default to gen3
+            robot_model_path = str(pkg / "urdf/gen3/robotont.urdf.xacro")
+
+    primary_rgba = _rgba_string_from_user_value(primary_in)
+
+    robot_description = ParameterValue(
+        Command([
+            "xacro ", robot_model_path,
+            ' main_color:="', primary_rgba, '"',
+        ]),
+        value_type=str,
+    )
+
+    rsp = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[{"robot_description": robot_description}],
+    )
+
+    jsp = Node(
+        package="joint_state_publisher",
+        executable="joint_state_publisher",
+    )
+
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+        arguments=[
+            "-d", LaunchConfiguration("rviz_config"),
+            "--fixed-frame", LaunchConfiguration("rviz_fixed_frame"),
+        ],
+    )
+
+    return [jsp, rsp, rviz]
+
+
+def generate_launch_description():
+    pkg = get_package_share_path("robotont_description")
+    default_rviz_config_path = pkg / "config/robotont_description.rviz"
+
+    return LaunchDescription([
+        DeclareLaunchArgument("model", default_value=""),
+        DeclareLaunchArgument("rviz_config", default_value=str(default_rviz_config_path)),
+        DeclareLaunchArgument("rviz_fixed_frame", default_value="base_link"),
+        DeclareLaunchArgument("generation", default_value="3"),
+
+        DeclareLaunchArgument("primary_color", default_value="0.16 0.65 0.98 1.0"),
+
+        OpaqueFunction(function=_setup),
+    ])

urdf/gen3/base.urdf.xacro

@@ -5,6 +5,7 @@
 
     <xacro:macro name="base_macro" params="
         color
+        spacers_color
         pcb_color
         e_button_color
         power_button_color
@@ -20,7 +21,7 @@
             <visual>
                 <origin rpy="0 0 ${pi/2}" xyz="0 0 ${frame_elevation}"/>
                 <geometry>
-                    <mesh filename="file://$(find robotont_description)/meshes/gen3/frame_module.STL"/>
+                    <mesh filename="file://$(find robotont_description)/meshes/gen3/frame_module_pla.STL"/>
                 </geometry>
                 <material name="base_material">
                     <color rgba="${color}"/>
@@ -48,7 +49,25 @@
         <joint name="${prefix}base_joint" type="fixed">
             <parent link="${prefix}base_footprint"/>
             <child link="${prefix}base_link"/>
-            <origin xyz="0 0 ${frame_elevation}" rpy="0 0 0"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+        </joint>
+
+        <link name="${prefix}base_link_spacers">
+            <visual>
+                <origin rpy="0 0 ${pi/2}" xyz="0 0 ${frame_elevation}"/>
+                <geometry>
+                    <mesh filename="file://$(find robotont_description)/meshes/gen3/frame_module_spacers.STL"/>
+                </geometry>
+                <material name="spacers_material">
+                    <color rgba="${spacers_color}"/>
+                </material>
+            </visual>
+        </link>
+
+        <joint name="${prefix}base_joint_spacers" type="fixed">
+            <parent link="${prefix}base_footprint"/>
+            <child link="${prefix}base_link_spacers"/>
+            <origin xyz="0 0 0" rpy="0 0 ${frame_elevation}"/>
         </joint>
 
         <!-- PCB -->
@@ -114,6 +133,28 @@
             />
         </joint>
 
+        <!-- Encoder button -->
+        <link name="${prefix}encoder_button_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotont_description)/meshes/gen3/encoder_button.STL"/>
+                </geometry>
+                <material name="encoder_button_material">
+                    <color rgba="${power_button_color}"/>
+                </material>
+            </visual>
+        </link>
+
+        <joint name="encoder_button_joint" type="fixed">
+            <parent link="${prefix}base_link"/>
+            <child link="${prefix}encoder_button_link"/>
+            <axis xyz="0 0 1"/>
+            <origin
+                xyz="-0.07 0.12 ${frame_ground_clearance + frame_height - 0.007}"
+                rpy="0 0 0"
+            />
+        </joint>
+
         <!-- Battery module -->
         <link name="${prefix}battery_module_link">
             <visual>
@@ -135,5 +176,27 @@
                 rpy="${pi / 2} 0 ${pi / 2}"
             />
         </joint>
+
+        <!-- OLED module -->
+        <link name="${prefix}screen_module_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotont_description)/meshes/gen3/ssd1306.STL"/>
+                </geometry>
+                <material name="screen_module_material">
+                    <color rgba="${battery_module_color}"/>
+                </material>
+            </visual>
+        </link>
+
+        <joint name="screen_module_joint" type="fixed">
+            <parent link="${prefix}base_link"/>
+            <child link="${prefix}screen_module_link"/>
+            <axis xyz="0 0 1"/>
+            <origin
+                xyz="-0.0325 0.1185 ${frame_ground_clearance + frame_height-0.0015}"
+                rpy="0 0 0"
+            />
+        </joint>
     </xacro:macro>
 </robot>

urdf/gen3/motor.urdf.xacro

@@ -23,7 +23,7 @@
         <link name="${prefix}motor_${name}_link">
             <visual>
                 <geometry>
-                    <mesh filename="file://$(find robotont_description)/meshes/gen3/motor_module.STL"/>
+                    <mesh filename="file://$(find robotont_description)/meshes/gen3/motor_module2.STL"/>
                 </geometry>
                 <material name="motor_${name}_material">
                     <color rgba="${color}"/>

urdf/gen3/robotont.urdf.xacro

@@ -7,6 +7,9 @@
     <xacro:arg name="prefix" default=""/>
     <xacro:property name="prefix" value="$(arg prefix)"/>
 
+    <xacro:arg name="main_color"  default="0.16 0.65 0.98 1.0"/>
+    <xacro:property name="primary_color" value="$(arg main_color)"/>
+
     <xacro:property name="wheel_distance" value="0.151"/>
     <xacro:property name="wheel_center_offset" value="0.014"/>
     <xacro:property name="wheel_center_height" value="0.021"/>
@@ -16,6 +19,7 @@
     <xacro:property name="wheel_length" value="${0.0225}"/>
 
     <xacro:property name="frame_height" value="0.065"/>
+    <xacro:property name="spacers_color" value="0.8 0.8 0.8 1.0"/>
     <xacro:property name="frame_ground_clearance" value="0.014"/>
     <xacro:property name="frame_thickness" value="0.000025"/>
     <xacro:property name="base_color" value="0.16 0.65 0.98 1.0"/>
@@ -28,7 +32,8 @@
 
     <!-- Frame module -->
     <xacro:base_macro
-        color="${base_color}"
+        color="${primary_color}"
+        spacers_color="${spacers_color}"
         pcb_color="${pcb_color}"
         e_button_color="${e_button_color}"
         power_button_color="${power_button_color}"