map: added tracking image to groundplane

src/sherpa/CMakeLists.txt

@@ -31,3 +31,4 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY urdf   DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY materials DESTINATION share/${PROJECT_NAME}/)

src/sherpa/launch/asd.launch.py

@@ -0,0 +1,123 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import Command
+
+def generate_launch_description():
+
+    ld = LaunchDescription()  
+
+    pkg_name = 'sherpa'
+    pkg_share = get_package_share_directory(pkg_name)
+    pkg_prefix = os.path.dirname(pkg_share) 
+
+    world_path = os.path.join(pkg_share, 'worlds', 'asd_course.world')
+    xacro_file = os.path.join(pkg_share, 'urdf', 'sherpa.xacro')
+
+    # create nodes
+
+    gz_env = SetEnvironmentVariable(
+        name='GZ_SIM_RESOURCE_PATH',
+        value=[pkg_prefix]
+    )
+
+    gz = ExecuteProcess(
+        cmd=[
+            'ros2', 'launch', 'ros_gz_sim', 'gz_sim.launch.py',
+            f'gz_args:=-r {world_path}'
+        ],
+        output='screen'
+    )
+
+    state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': Command(['xacro ', xacro_file])
+        }]
+    )
+
+    controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['ackermann_controller'],
+        #output='screen'
+    )
+
+    joint_state_broadcaster = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+        parameters=[{'use_sim_time': True}],
+        #output='screen'
+    )
+
+    static_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_tf_base_to_lidar',
+        arguments=[
+            '0', '0', '0',
+            '0', '0', '0',
+            'velodyne',
+            'lidar_link',
+        ]          
+    )
+
+    robot = ExecuteProcess(
+        cmd=[
+            'ros2', 'run', 'ros_gz_sim', 'create',
+            '-name', 'sherpa',
+            '-topic', 'robot_description',
+            '-x', '0', '-y', '0', '-z', '2'
+        ],
+        output='screen'
+    )
+
+    twist_stamper = Node(
+        package="twist_stamper",
+        executable="twist_stamper",
+        remappings=[
+            ('/cmd_vel_out', '/ackermann_controller/reference'),
+        ],
+        output='screen'
+    )
+
+    gz_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=[
+            "--ros-args",
+            "-p",
+            f"config_file:={os.path.join(pkg_share, 'config', 'gz_bridge.yaml')}",
+        ]
+    )
+
+
+    rviz2 = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', f"{os.path.join(get_package_share_directory('sherpa'), 'config', 'sherpa.rviz')}"]
+    )
+
+    ld.add_action(gz_env)
+    ld.add_action(gz)
+    ld.add_action(state_publisher)
+    ld.add_action(static_tf)
+    ld.add_action(robot)
+    ld.add_action(gz_bridge)
+    ld.add_action(joint_state_broadcaster)
+    ld.add_action(controller)
+    ld.add_action(twist_stamper)
+    ld.add_action(rviz2)
+
+    return ld

src/sherpa/urdf/components/camera.xacro

@@ -31,7 +31,7 @@
                     <image>
                         <width>800</width>
                         <height>800</height>
-                        <format>R8G8B8</format>
+                        <format>L8</format>
                     </image>
                     <clip>
                         <near>0.02</near>

src/sherpa/urdf/sherpa.xacro

@@ -25,7 +25,7 @@
     <xacro:include filename="components/wheel.xacro"/>
     <xacro:include filename="components/ouster_lidar.xacro"/>
 
-    <xacro:camera name="front_camera" parent="base" xyz="0 0 2" rpy="0 0 0" frequency="30" fov="90" topic="/front_camera"/>
+    <xacro:camera name="front_camera" parent="base" xyz="0 0 0.75" rpy="0 0 0" frequency="30" fov="90" topic="/front_camera"/>
     <xacro:imu name="imu" parent="base" xyz="0 0 0.35" rpy="0 0.05 0" frequency="300" topic="/imu"/>
     <xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.35" rpy="0 0.05 0" model="vlp16" frequency="10" topic="/velodyne_points"/>
 

src/sherpa/worlds/asd_course.world

@@ -0,0 +1,170 @@
+<?xml version="1.0"?>
+<sdf version="1.7">
+    <world name="default">
+
+        <!-- A simple light -->
+        <light name="sun" type="directional">
+            <pose>0 0 20 0 0 0</pose>
+            <diffuse>0.8 0.8 0.8 1.0</diffuse>
+            <specular>0.8 0.8 0.8 1.0</specular>
+            <attenuation>
+                <range>1000</range>
+                <constant>0.9</constant>
+                <linear>0.05</linear>
+                <quadratic>0.01</quadratic>
+            </attenuation>
+            <cast_shadows>false</cast_shadows>
+        </light>
+        <scene>
+            <grid>false</grid>
+        </scene>
+        <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
+        </plugin>
+        <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+            <render_engine>ogre2</render_engine>
+        </plugin>
+        <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu">
+        </plugin>
+        <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
+        </plugin>
+        <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
+        </plugin>
+
+        <gui fullscreen="0">
+
+            <!-- 3D scene -->
+            <plugin filename="GzScene3D" name="3D View">
+                <ignition-gui>
+                    <title>3D View</title>
+                    <property type="bool" key="showTitleBar">false</property>
+                    <property type="string" key="state">docked</property>
+                </ignition-gui>
+
+                <engine>ogre2</engine>
+                <scene>scene</scene>
+                <ambient_light>0.4 0.4 0.4</ambient_light>
+                <background_color>0.5 0.8 0.95</background_color>
+                <!--camera_pose>276.46 -130.36 5.27 0 0.43 -1.24</camera_pose-->
+                <camera_follow>
+                    <target>prius_hybrid</target>
+                    <p_gain>0.5</p_gain>
+                    <offset>0.5 5 2</offset>
+                </camera_follow>
+            </plugin>
+
+            <!-- World control -->
+            <plugin filename="WorldControl" name="World control">
+                <ignition-gui>
+                    <title>World control</title>
+                    <property type="bool" key="showTitleBar">false</property>
+                    <property type="bool" key="resizable">false</property>
+                    <property type="double" key="height">72</property>
+                    <property type="double" key="width">121</property>
+                    <property type="double" key="z">1</property>
+
+                    <property type="string" key="state">floating</property>
+                    <anchors target="3D View">
+                        <line own="left" target="left"/>
+                        <line own="bottom" target="bottom"/>
+                    </anchors>
+                </ignition-gui>
+
+                <play_pause>true</play_pause>
+                <step>true</step>
+                <start_paused>true</start_paused>
+
+            </plugin>
+
+            <!-- World statistics -->
+            <plugin filename="WorldStats" name="World stats">
+                <ignition-gui>
+                    <title>World stats</title>
+                    <property type="bool" key="showTitleBar">false</property>
+                    <property type="bool" key="resizable">false</property>
+                    <property type="double" key="height">110</property>
+                    <property type="double" key="width">290</property>
+                    <property type="double" key="z">1</property>
+
+                    <property type="string" key="state">floating</property>
+                    <anchors target="3D View">
+                        <line own="right" target="right"/>
+                        <line own="bottom" target="bottom"/>
+                    </anchors>
+                </ignition-gui>
+
+                <sim_time>true</sim_time>
+                <real_time>true</real_time>
+                <real_time_factor>true</real_time_factor>
+                <iterations>true</iterations>
+            </plugin>
+
+            <plugin filename="ImageDisplay" name="dis">
+                <ignition-gui>
+                    <property key="state" type="string">docked</property>
+                </ignition-gui>
+                <topic>front_camera</topic>
+            </plugin>
+
+            <plugin filename="Teleop" name="Teleop">
+                <ignition-gui>
+                    <property type="string" key="state">docked</property>
+                </ignition-gui>
+                <topic>/cmd_vel</topic>
+            </plugin>
+        </gui>
+
+        <model name="carolo_track">
+            <static>true</static>
+            <link name="link">
+                <collision name="collision">
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>200 200</size>
+                        </plane>
+                    </geometry>
+                    <surface>
+                        <friction>
+                            <ode>
+                                <mu>50</mu>
+                            </ode>
+                        </friction>
+                    </surface>
+                </collision>
+                <visual name="visual">
+                    <cast_shadows>false</cast_shadows>
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>35 35</size>
+                        </plane>
+                    </geometry>
+                    <material>
+                        <pbr>
+                            <metal>
+                                <albedo_map>file://sherpa/materials/textures/track.png</albedo_map>
+                            </metal>
+                        </pbr>
+                        <ambient>1 1 1 1</ambient>
+                        <diffuse>1 1 1 1</diffuse>
+                    </material>
+                </visual>
+                <pose>0 0 0 0 0 0</pose>
+                <inertial>
+                    <pose>0 0 0 0 0 0</pose>
+                    <mass>1</mass>
+                    <inertia>
+                        <ixx>1</ixx>
+                        <ixy>0</ixy>
+                        <ixz>0</ixz>
+                        <iyy>1</iyy>
+                        <iyz>0</iyz>
+                        <izz>1</izz>
+                    </inertia>
+                </inertial>
+            </link>
+            <pose>0 0 0 0 0 0</pose>
+            <self_collide>false</self_collide>
+        </model>
+    </world>
+</sdf>