Lots of cleanup and changes to try and get the simplified model to wo…

…rk. Still won't respond to angular velocity for some reason

urc_bringup/config/ros2_control_walli.yaml

@@ -1,6 +1,7 @@
 controller_manager:
   ros__parameters:
     update_rate: 200
+    use_sim_time: true
 
     imu_broadcaster:
       type: urc_controllers/IMUBroadcaster
@@ -37,6 +38,7 @@ status_light_controller:
   ros__parameters:
     status_light_name: status_light
 
+# For more information on each of these parameters, see https://github.com/ros-controls/ros2_controllers/blob/master/diff_drive_controller/src/diff_drive_controller_parameter.yaml
 rover_drivetrain_controller:
   ros__parameters:
     left_wheel_names:
@@ -52,35 +54,25 @@ rover_drivetrain_controller:
         "right_rear_wheel_joint",
       ]
 
-    pose_covariance_diagonal:
-      [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
-    twist_covariance_diagonal:
-      [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
 
-    position_feedback: false
-    open_loop: false
+    wheel_seperation: 0.4
+    wheels_per_side: 3
+    wheel_radius: 0.10
 
-    wheel_seperation: 0.5
+    use_stamped_vel: false
+
+    open_loop: true
+    enable_odom_tf: true
 
-    wheel_radius: 0.20
     cmd_vel_timeout: 0.5
-    # Velocity and acceleration limits
-    # Whenever a min_* is unspecified, default to -max_*
-    linear:
-      x:
-        has_velocity_limits: true
-        max_velocity: 4.0 # m/s
-        min_velocity: -0.5 # m/s
-        has_acceleration_limits: true
-        max_acceleration: 0.8 # m/s^2
-        min_acceleration: -0.4 # m/s^2
-        has_jerk_limits: true
-        max_jerk: 5.0 # m/s^3
-    angular:
-      z:
-        has_velocity_limits: true
-        max_velocity: 1.7 # rad/s
-        has_acceleration_limits: true
-        max_acceleration: 1.5 # rad/s^2
-        has_jerk_limits: true
-        max_jerk: 2.5 # rad/s^3
+
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0

urc_bringup/launch/bringup.launch.py

@@ -1,47 +1,51 @@
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
-from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution
 from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration, Command
-from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from ament_index_python.packages import get_package_share_directory
 import os
 import yaml
-import launch_ros
-import launch
 
 
 def generate_launch_description():
 
     pkg_urc_bringup = FindPackageShare("urc_bringup").find("urc_bringup")
     pkg_urc_navigation = FindPackageShare("urc_navigation").find("urc_navigation")
 
-    world_path = os.path.join(pkg_urc_bringup, 'world/world.sdf')
-    default_model_path = os.path.join(get_package_share_directory('urc_hw_description'), "urdf/robot.xacro")
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("urc_hw_description"), "urdf", "robot.xacro"]
+            ),
+        ]
+    )
 
-    hardware_config_file_dir = os.path.join(pkg_urc_bringup, 'config', 'hardware_config.yaml')
-    with open(hardware_config_file_dir) as f:
+    hardware_config_filepath = os.path.join(pkg_urc_bringup, 'config', 'hardware_config.yaml')
+    with open(hardware_config_filepath) as f:
         hardware_config = yaml.safe_load(f)
+
     use_simulation = hardware_config['hardware_config']['use_simulation']
 
-    controller_config_file_dir = os.path.join(
+    robot_controllers_config = os.path.join(
         pkg_urc_bringup, 'config', 'ros2_control_walli.yaml'
     )
 
     default_rviz_config_path = os.path.join(
-        pkg_urc_bringup, 'rviz/urdf_config.rviz'
+        pkg_urc_bringup, 'rviz', 'urdf_config.rviz'
+    )
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [PathJoinSubstitution([FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"])]
+        ),
+        launch_arguments={"verbose": "false"}.items(),
     )
 
-    # gazebo = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource(
-    #         os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py'),
-    #     ),
-    #     launch_arguments={"world": world_path}.items()
-    # )
 
-    navigation_nodes = IncludeLaunchDescription(
+    launch_navigation = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_urc_navigation, "launch", "navigation.launch.py")
         )
@@ -50,65 +54,31 @@ def generate_launch_description():
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[controller_config_file_dir],
+        parameters=[robot_controllers_config],
         output="both",
     )
+
     enable_color = SetEnvironmentVariable(
         name="RCUTILS_COLORIZED_OUTPUT",
         value="1"
     )
-    aruco_detector = Node(
-        package='urc_perception',
-        executable='urc_perception_ArucoDetector',
-        output='screen',
-        parameters=[
-                PathJoinSubstitution([
-                    FindPackageShare('urc_perception'),
-                    'config', 'aruco_detector_params.yaml'])
-        ],
-        remappings=[
-            ("/aruco_detector/aruco_detection", "/aruco_detection")
-        ]
-    )
-    aruco_location = Node(
-        package='urc_perception',
-        executable='urc_perception_ArucoLocation',
-        output='screen',
-        remappings=[
-                ("/aruco_location/aruco_location", "/aruco_location")
-        ]
-    )
+
     spawn_robot = Node(
         package='gazebo_ros',
         executable='spawn_entity.py',
         arguments=['-entity', 'walli',
                    '-x', '0', '-y', '0', '-z', '0.4',
-                   '-R', '0', '-P', '0', '-Y', '0',
                    '-topic', '/robot_description'],
         output='screen'
     )
+
     robot_state_publisher_node = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
         name='robot_state_publisher',
-        parameters=[
-            # {"use_sim_time": LaunchConfiguration('use_sim_time')},
-            {"robot_description": Command(
-                ['xacro ', LaunchConfiguration('model')])}],
+        parameters=[{"robot_description": robot_description_content}],
         output='screen'
     )
-    joint_state_publisher_node = launch_ros.actions.Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher',
-        condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
-    )
-    joint_state_publisher_gui_node = launch_ros.actions.Node(
-        package='joint_state_publisher_gui',
-        executable='joint_state_publisher_gui',
-        name='joint_state_publisher_gui',
-        condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
-    )
 
     rviz_node = Node(
         package='rviz2',
@@ -121,18 +91,13 @@ def generate_launch_description():
     load_joint_state_broadcaster = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=[
-            '-p', controller_config_file_dir,
-            'joint_state_broadcaster'
-        ],
+        arguments=["joint_state_broadcaster"],
     )
 
     load_drivetrain_controller = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=[
-            'rover_drivetrain_controller'
-        ],
+        arguments=['rover_drivetrain_controller'],
     )
 
     joystick_launch = IncludeLaunchDescription(
@@ -143,19 +108,16 @@ def generate_launch_description():
             ]
         )
     )
+
     if use_simulation:
-        print("USING SIMULATION MODE")
+        print("Using simulation...")
+
         return LaunchDescription([
             DeclareLaunchArgument(
                 name='rvizconfig',
                 default_value=default_rviz_config_path,
                 description='Absolute path to rviz config file'
             ),
-            DeclareLaunchArgument(
-                name='model',
-                default_value=default_model_path,
-                description='Absolute path to robot urdf file'
-            ),
             DeclareLaunchArgument(
                 name='use_sim_time',
                 default_value='True',
@@ -166,31 +128,20 @@ def generate_launch_description():
                 default_value='True',
                 description='Flag to enable joint_state_publisher_gui'
             ),
-            ExecuteProcess(cmd=[
-                'gazebo', '--verbose', '-s',
-                'libgazebo_ros_init.so', '-s',
-                'libgazebo_ros_factory.so', world_path
-                ],
-                output='screen'
-            ),
-
             enable_color,
-            # gazebo,
+            gazebo,
             robot_state_publisher_node,
-            joint_state_publisher_node,
-            joint_state_publisher_gui_node,
-            load_drivetrain_controller,
             spawn_robot,
+            load_joint_state_broadcaster,
+            load_drivetrain_controller,
             rviz_node,
-            navigation_nodes
+            # launch_navigation
         ])
     else:
         return LaunchDescription([
             robot_state_publisher_node,
             control_node,
             load_joint_state_broadcaster,
             load_drivetrain_controller,
-            aruco_detector,
-            aruco_location,
             joystick_launch
         ])

urc_bringup/launch/heartbeat_publisher.launch.py

@@ -16,6 +16,7 @@ def generate_launch_description():
                     'heartbeat_publisher.yaml'])
             ]
         )
+
     return LaunchDescription([
         heartbeat_publisher_node
     ])

urc_hw_description/urdf/robot.xacro

@@ -10,7 +10,8 @@
         value="${config_data['hardware_config']['use_simplified_model']}" />
 
     <xacro:if value="${use_simplified_model}">
-        <xacro:include filename="$(find urc_hw_description)/urdf/six_wheel_simplified.xacro" />
+        <xacro:include
+            filename="$(find urc_hw_description)/urdf/simplified_model/simplified_model.xacro" />
     </xacro:if>
 
     <xacro:if value="${not use_simplified_model}">

urc_hw_description/urdf/simplified_model/inertial_macros.xacro

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <!-- Define some commonly used intertial properties  -->
+    <xacro:macro name="box_inertia" params="m w h d">
+        <inertial>
+            <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}" />
+            <mass value="${m}" />
+            <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}"
+                iyz="0.0" izz="${(m/12) * (w*w + h*h)}" />
+        </inertial>
+    </xacro:macro>
+
+    <xacro:macro name="cylinder_inertia" params="m r h">
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <mass value="${m}" />
+            <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0.0" ixz="0.0"
+                iyy="${(m/12) * (3*r*r + h*h)}"
+                iyz="0.0" izz="${(m/2) * (r*r)}" />
+        </inertial>
+    </xacro:macro>
+
+    <xacro:macro name="sphere_inertia" params="m r">
+        <inertial>
+            <mass value="${m}" />
+            <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}"
+                iyz="0.0" izz="${(2/5) * m * (r*r)}" />
+        </inertial>
+    </xacro:macro>
+
+
+</robot>