<ros2-humble> Fixed issues + Scout v2 moves on Gazebo Classic

README.md

@@ -44,3 +44,19 @@ This repository contains minimal packages to control the scout robot using ROS.
     ```
     $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
     ```
+
+## To run Scout2 on Gazebo 
+Launch the launch file `display_scout.launch.py`
+
+```
+ros2 launch scout_description display_scout.launch.py
+```
+
+
+To run generic Keyboard Teleop, Run
+```ros2 run teleop_twist_keyboard teleop_twist_keyboard```
+
+[scout_ros2_OS1.webm](https://github.com/arif09/scout_ros2/assets/5939058/4b19ce6f-120d-4e88-8862-61092535024f)
+
+
+

scout_base/CMakeLists.txt

@@ -34,6 +34,10 @@ include_directories(
   include
 )
 
+include_directories(/opt/ros/humble/include/tf2_geometry_msgs)
+include_directories(/opt/ros/humble/include/tf2)
+include_directories(/opt/ros/humble/include/tf2_ros)
+
 set(DEPENDENCIES
   "geometry_msgs"
   "nav_msgs"

scout_base/include/scout_base/scout_messenger.hpp

@@ -18,7 +18,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <tf2_ros/transform_broadcaster.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include "scout_msgs/msg/scout_status.hpp"
 #include "scout_msgs/msg/scout_light_cmd.hpp"

scout_base/src/scout_base_ros.cpp

@@ -15,17 +15,20 @@
 namespace westonrobot {
 ScoutBaseRos::ScoutBaseRos(std::string node_name)
     : rclcpp::Node(node_name), keep_running_(false) {
-  this->declare_parameter("port_name");
+
+ auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
+  param_desc.description = "Param Desc. ";
 
-  this->declare_parameter("odom_frame");
-  this->declare_parameter("base_frame");
-  this->declare_parameter("odom_topic_name");
+  this->declare_parameter("port_name", port_name_, param_desc, false);
+  this->declare_parameter("odom_frame", odom_frame_, param_desc, false);
+  this->declare_parameter("base_frame", base_frame_, param_desc, false);
+  this->declare_parameter("odom_topic_name", odom_topic_name_, param_desc, false);
 
-  this->declare_parameter("is_scout_mini");
-  this->declare_parameter("is_omni_wheel");
+  this->declare_parameter("is_scout_mini", is_scout_mini_, param_desc, false);
+  this->declare_parameter("is_omni_wheel", is_omni_wheel_, param_desc, false);
 
-  this->declare_parameter("simulated_robot");
-  this->declare_parameter("control_rate");
+  this->declare_parameter("simulated_robot", simulated_robot_, param_desc, false);
+  this->declare_parameter("control_rate", sim_control_rate_, param_desc, false);
 
   LoadParameters();
 }

scout_description/CMakeLists.txt

@@ -16,6 +16,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(rviz_common REQUIRED)
 
 install(DIRECTORY launch urdf meshes
   DESTINATION share/${PROJECT_NAME}/)

scout_description/launch/display_scout.launch.py

@@ -0,0 +1,126 @@
+import os
+import launch
+import launch_ros
+
+from ament_index_python.packages import get_package_share_directory
+
+from math import pi
+
+from launch.conditions import IfCondition, UnlessCondition
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import FindExecutable, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, Command
+from launch_ros.actions import Node
+from launch.event_handlers import OnProcessExit
+
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+
+
+def generate_launch_description():
+    model_name = 'scout2.urdf.xacro'
+    model_path = os.path.join(get_package_share_directory('scout_description'), "urdf", model_name)
+
+    print(model_path)
+
+
+    robot_description_content = Command([
+        PathJoinSubstitution([FindExecutable(name="xacro")]), " ",
+        PathJoinSubstitution(
+            [FindPackageShare("scout_description"), "urdf", model_name]
+        ),
+    ])
+
+    robot_description = {"robot_description": robot_description_content}
+
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("scout_description"), "rviz", "model_display.rviz"]
+    )
+
+
+    rviz_node = Node(
+                package="rviz2",
+                executable="rviz2",
+                name="rviz2",
+                output="log",
+                # arguments=["-d", rviz_config_file],
+            )
+
+
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters= [{"use_sim_time": True}, robot_description], #[{"use_sim_time": True}, robot_description],
+
+    )
+
+    joint_state_publisher_node = launch_ros.actions.Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        parameters=[{'use_sim_time': True}],
+    )
+
+    pointcloud_to_laserscan_node = Node(
+		package='pointcloud_to_laserscan',
+		executable='pointcloud_to_laserscan_node',
+		name='pointcloud_to_laserscan_node',
+		remappings=[('cloud_in', "/ray/pointcloud2"),
+					('scan', "/scan")],
+		parameters=[{
+			'transform_tolerance': 0.05,
+			'min_height': 0.0,
+			'max_height': 1.0,
+			'angle_min': -pi,
+			'angle_max': pi,
+			'angle_increment': pi / 180.0 / 2.0,
+			'scan_time': 1/10, # 10Hz
+			'range_min': 0.1,
+			'range_max': 100.0,
+			'use_inf': True,
+		}],
+	)
+
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"]
+        ),
+    )
+
+    # Spawn robot
+    gazebo_spawn_robot = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        name="spawn_scout",
+        arguments=["-entity", "scout2", 
+                   "-topic", "robot_description"],
+        output="screen",
+    )
+
+
+    return launch.LaunchDescription([
+        DeclareLaunchArgument('use_sim_time', default_value='true',
+            description='Use simulation clock if true'),
+
+        launch.actions.LogInfo(msg='use_sim_time: '),
+        launch.actions.LogInfo(msg=launch.substitutions.LaunchConfiguration('use_sim_time')),
+
+            robot_state_publisher_node,
+            joint_state_publisher_node,
+            rviz_node,
+            gazebo,
+            gazebo_spawn_robot,
+            pointcloud_to_laserscan_node,
+
+    ])

scout_description/launch/scout_base_description.launch.py

@@ -12,7 +12,7 @@
 
 
 def generate_launch_description():
-    model_name = 'scout_v2.xacro'
+    model_name = 'scout2.urdf.xacro'
     # model_path = os.path.join(get_package_share_directory('scout_description'), "urdf", model_name)
     # print(model_path)
     robot_description_content = Command([

scout_description/urdf/gazebo_control.xacro

@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <gazebo>
+        <plugin name='skid_steer_drive'  filename='libgazebo_ros_diff_drive.so'>
+
+            <updateRate>500</updateRate>
+
+            <num_wheel_pairs>2</num_wheel_pairs>
+            <left_joint>front_left_wheel</left_joint>
+            <right_joint>front_right_wheel</right_joint>
+
+            <left_joint>rear_left_wheel</left_joint>
+            <right_joint>rear_right_wheel</right_joint>
+
+            <wheel_separation>1.35</wheel_separation>
+            <wheel_separation>1.35</wheel_separation>
+
+            <wheel_diameter>0.3</wheel_diameter>
+            <wheel_diameter>0.3</wheel_diameter>
+
+            <use_stamped_vel>false</use_stamped_vel>
+
+            <!-- Limits -->
+            <max_wheel_torque>200</max_wheel_torque>
+            <max_wheel_acceleration>10.0</max_wheel_acceleration>
+
+
+            <!-- Output -->
+            <odometry_frame>odom</odometry_frame>
+            <robot_base_frame>base_link</robot_base_frame>
+
+            <publish_odom>true</publish_odom>
+            <publish_odom_tf>true</publish_odom_tf>
+            <publish_wheel_tf>true</publish_wheel_tf>
+
+        </plugin>
+    </gazebo>
+
+
+</robot>

scout_description/urdf/ouster.xacro

@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
+
+    <joint name="laser_joint" type="fixed">
+        <parent link="base_link"/>
+        <child link="laser_frame"/>
+        <origin xyz="0 0 0.12" rpy="0 0 0"/>
+    </joint>
+
+    <link name="laser_frame">
+      <inertial>
+        <mass value="0.425"/>
+        <origin xyz="0 0 0.003675" rpy="0 0 0" />
+        <inertia ixx="0.000308429" ixy="0" ixz="0"
+                 iyy="0.000308429" iyz="0" izz="0.00034589"/>
+      </inertial>
+      <collision name="lidar_collision">
+        <origin xyz="0 0 0.03675" rpy="0 0 0" />
+        <geometry>
+          <cylinder radius="0.04350" length="0.07350"/>
+        </geometry>
+      </collision>
+      <visual>
+            <geometry>
+                <cylinder radius="0.01" length="0.23"/>
+            </geometry>
+
+      </visual>
+
+      <visual name="lidar_visual">
+        <origin xyz="0 0 0.10" rpy="0 0 0"/>
+        <geometry>
+           <mesh filename="file://$(find scout_description)/meshes/os1_64.stl" />
+        </geometry>
+      </visual>
+    </link>
+
+
+
+    <gazebo reference="laser_frame">
+        <material>Gazebo/DarkGrey</material>
+
+        <sensor name="sensor_ray" type="ray">
+             <pose> 0 0 0 0 0 0 </pose>
+            <visualize>false</visualize>
+            <update_rate>10</update_rate>
+            <ray>
+            <scan>
+              <horizontal>
+                <samples>360</samples>
+                <resolution>1.0</resolution>
+                 <min_angle>-3.14</min_angle>
+                <max_angle>3.14</max_angle>
+              </horizontal>
+              <vertical>
+                <samples>100</samples>
+                <resolution>1.0</resolution>
+                <min_angle>-0.28</min_angle>
+                <max_angle>0.28</max_angle>
+              </vertical>
+            </scan>
+            <range>
+              <min>0.05</min>
+              <max>15.0</max>
+            </range>
+          </ray>
+          <always_on>true</always_on>
+          <update_rate>1.0</update_rate>
+            <plugin name="pc2" filename="libgazebo_ros_ray_sensor.so">
+                 <ros>
+                    <namespace>/ray</namespace>
+                    <remapping>~/out:=pointcloud2</remapping>
+                </ros>
+                <output_type>sensor_msgs/PointCloud2</output_type>
+                <frame_name>laser_frame</frame_name>
+            </plugin>
+             <!-- <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
+                 <ros>
+                    <namespace>/ray</namespace>
+                    <remapping>~/out:=laserscan</remapping>
+                </ros>
+                <output_type>sensor_msgs/LaserScan</output_type>
+                <frame_name>laser_frame</frame_name>
+            </plugin> -->
+        </sensor>
+    </gazebo>
+
+
+
+</robot>

scout_description/urdf/scout2.urdf.xacro

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"  name="scout">
+
+    <xacro:include filename="scout_v2.xacro" />
+    <xacro:include filename="ouster.xacro" />
+    <xacro:include filename="gazebo_control.xacro" />
+
+</robot>