diff --git a/.gitignore b/.gitignore
index c35ace7c..2f2d9223 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,8 @@
devel/
bin/
+build/
+install/
+log/
msg_gen/
srv_gen/
msg/*Action.msg
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py
index a8e71f59..c89da6f1 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py
@@ -3,57 +3,115 @@
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-
from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
import xacro
-def generate_launch_description():
-
- # Specify the name of the package and path to xacro file within the package
- pkg_name = 'uwrt_mars_rover_drivetrain_description'
- file_subpath = 'urdf/drivetrain.urdf.xacro'
-
- pkg_share = FindPackageShare(package=pkg_name).find(pkg_name)
- world_path=os.path.join(pkg_share, 'world/my_world.sdf')
-
- # Use xacro to process the file
- xacro_file = os.path.join(get_package_share_directory(pkg_name),file_subpath)
- robot_description_raw = xacro.process_file(xacro_file).toxml()
-
-
- # Configure the node
- node_robot_state_publisher = Node(
- package='robot_state_publisher',
- executable='robot_state_publisher',
- output='screen',
- parameters=[{'robot_description': robot_description_raw,
- 'use_sim_time': True}] # add other parameters here if required
- )
-
+from ament_index_python.packages import get_package_share_path
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command
+from launch_ros.parameter_descriptions import ParameterValue
+def generate_launch_description():
+ #this is launching gazebo
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
)
-
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
- arguments=['-topic', 'robot_description',
- '-entity', 'my_bot'],
- output='screen')
+ arguments=['-topic', 'robot_description',
+ '-entity', 'my_bot'],
+ output='screen')
+ #-------------------------------------------------------------------------------------
+
+
+ drivetrain_description_package_path = get_package_share_path('uwrt_mars_rover_drivetrain_description')
+ model_path = drivetrain_description_package_path / 'urdf' / 'drivetrain.urdf.xacro'
+ rviz_config_path = drivetrain_description_package_path / 'rviz' / 'urdf.rviz'
+ controllers_config_path = get_package_share_path(
+ 'uwrt_mars_rover_drivetrain_hw') / 'config' / 'drivetrain_controllers.yaml'
+
+ robot_description_content = ParameterValue(Command(['ros2 run xacro xacro ', str(model_path)]), value_type=str)
+ robot_description = {'robot_description': robot_description_content}
+
+ # Nodes
+ nodes = []
+ nodes += [gazebo]
+ nodes += [spawn_entity]
+
+ nodes += [Node(
+ package='controller_manager',
+ executable='ros2_control_node',
+ parameters=[robot_description, controllers_config_path],
+ output={
+ 'stdout': 'screen',
+ 'stderr': 'screen',
+ },
+ )] # TODO: use custom control node w/ RT scheduling(port from ros1 uwrt_mars_rover branch)
+
+ nodes += [Node(
+ package='controller_manager',
+ executable='ros2_control_node',
+ parameters=[robot_description, controllers_config_path],
+ output={
+ 'stdout': 'screen',
+ 'stderr': 'screen',
+ },
+ )] # TODO: use custom control node w/ RT scheduling(port from ros1 uwrt_mars_rover branch)
+
+ nodes += [Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ output='both',
+ parameters=[robot_description],
+ remappings=[
+ ("/differential_drivetrain_controller/cmd_vel_unstamped", "/cmd_vel"),
+ ],
+ )]
+
+ nodes += [joint_state_broadcaster_spawner := Node(
+ package='controller_manager',
+ executable='spawner',
+ arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+ )]
+
+ # Delay rviz2 start after joint_state_broadcaster_spawner finishes
+ rviz_node = Node(
+ package='rviz2',
+ executable='rviz2',
+ name='rviz2',
+ output='',
+ arguments=['-d', str(rviz_config_path)],
+ )
+ nodes += [RegisterEventHandler(
+ event_handler=OnProcessExit(
+ target_action=joint_state_broadcaster_spawner,
+ on_exit=[rviz_node],
+ )
+ )]
+ # Delay start of drivetrain_controller_spawner after joint_state_broadcaster_spawner
+ drivetrain_controller_spawner = Node(
+ package='controller_manager',
+ executable='spawner',
+ arguments=['differential_drivetrain_controller', "--controller-manager", '/controller_manager'],
+ )
+ nodes += [RegisterEventHandler(
+ event_handler=OnProcessExit(
+ target_action=joint_state_broadcaster_spawner,
+ on_exit=[drivetrain_controller_spawner],
+ )
+ )]
+ #-------------------------------------------------------------------------------------
# Run the node
- return LaunchDescription([
- gazebo,
- node_robot_state_publisher,
- spawn_entity
- ])
+ return LaunchDescription(
+ nodes
+ )
\ No newline at end of file
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo2.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo2.launch.py
new file mode 100644
index 00000000..9a4595d8
--- /dev/null
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo2.launch.py
@@ -0,0 +1,94 @@
+import os
+from ament_index_python import get_package_share_path
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler,TimerAction,DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.event_handlers import OnProcessExit, OnExecutionComplete
+from launch.substitutions import LaunchConfiguration, Command
+
+
+from launch_ros.actions import Node
+import xacro
+
+
+def generate_launch_description():
+ # Specify the name of the package and path to xacro file within the package
+ pkg_name = 'uwrt_mars_rover_drivetrain_description'
+ file_subpath = 'urdf/drivetrain.urdf.xacro'
+ controllers_config_path = get_package_share_path(
+ 'uwrt_mars_rover_drivetrain_hw') / 'config' / 'drivetrain_controllers.yaml'
+
+ sim_status = LaunchConfiguration('use_sim')
+
+
+ # Use xacro to process the file
+ xacro_file = os.path.join(get_package_share_directory(pkg_name),file_subpath)
+
+ robot_description_config = Command(['xacro ', xacro_file,' sim:=', 'true'])
+
+
+ # Configure the node
+ params={'robot_description': robot_description_config}
+ node_robot_state_publisher = Node(
+ package='robot_state_publisher',
+ executable='robot_state_publisher',
+ name='robot_state_publisher',
+ output='screen',
+ parameters=[params] # add other parameters here if required
+ )
+
+
+ gazebo = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource([os.path.join(
+ get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+ )
+
+
+ spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+ arguments=['-topic', 'robot_description',
+ '-entity', 'my_bot'],
+ output='screen')
+
+
+ differential_drivetrain_controller = ExecuteProcess(
+ cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+ 'differential_drivetrain_controller'],
+ output='screen'
+ )
+
+ joint_state_broadcaster_controller = ExecuteProcess(
+ cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+ 'joint_state_broadcaster'],
+ output='screen'
+ )
+
+
+ # Run the node
+ return LaunchDescription([
+
+ DeclareLaunchArgument(
+ 'use_sim',
+ default_value='true',
+ description='Use ros2_control if true'),
+ RegisterEventHandler(
+ OnExecutionComplete(
+ target_action=spawn_entity,
+ on_completion=[
+ TimerAction(
+ period=5.0,
+ actions=[joint_state_broadcaster_controller],
+ )
+ ]
+ )
+ ),
+ RegisterEventHandler(
+ event_handler=OnProcessExit(
+ target_action=joint_state_broadcaster_controller,
+ on_exit=[differential_drivetrain_controller],
+ )
+ ),
+ gazebo,
+ spawn_entity,
+ node_robot_state_publisher
+ ])
\ No newline at end of file
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml
index 229c4cca..31f88e26 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml
@@ -22,6 +22,24 @@
xacro
sensor_msgs
+
+ controller_manager
+ diff_drive_controller
+ joint_state_broadcaster
+ robot_state_publisher
+
+
+ ament_index_python
+ control_msgs
+ diff_drive_controller
+
+
+
+ gazebo_ros2_control
+ gazebo_ros
+ ros2_control
+ ros2_controllers
+
ament_cmake_flake8
ament_cmake_xmllint
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
index cfc26de4..c43b8176 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
@@ -69,7 +69,7 @@
-
+
@@ -91,6 +91,20 @@
+
+
+
+
+
+ ${-max_speed}
+ ${max_speed}
+
+
+
+
+
+
+
@@ -110,13 +124,13 @@
-
-
- transmission_interface/SimpleTransmission
-
- ${mechanical_reduction}
-
-
+
+
+
+
+
+
+
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2control.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2control.xacro
index 2fa6841d..8b4d68f6 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2control.xacro
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2control.xacro
@@ -3,13 +3,13 @@
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
-
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2controlgazebo.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2controlgazebo.xacro
new file mode 100644
index 00000000..ac581883
--- /dev/null
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2controlgazebo.xacro
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+ gazebo_ros2_control/GazeboSystem
+
+
+
+
+
+
+
+
+
+
+
+
+
+ $(find uwrt_mars_rover_drivetrain_hw)/config/drivetrain_controllers.yaml
+ /robot_state_publisher
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
index 897b1cdd..dedf07a0 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.urdf.xacro
@@ -4,6 +4,8 @@
+
+
@@ -13,16 +15,19 @@
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
+
@@ -35,6 +40,14 @@
+
+
+
+
+
+
+
+
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml
index f61b24d4..7bc23a44 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/config/drivetrain_controllers.yaml
@@ -1,7 +1,7 @@
controller_manager:
ros__parameters:
update_rate: 100 # Hz
-
+ use_sim_time: true
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/launch/drivetrain.launch.py b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/launch/drivetrain.launch.py
index a387e2be..840f2f0e 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/launch/drivetrain.launch.py
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/launch/drivetrain.launch.py
@@ -23,7 +23,7 @@ def generate_launch_description():
controllers_config_path = get_package_share_path(
'uwrt_mars_rover_drivetrain_hw') / 'config' / 'drivetrain_controllers.yaml'
- robot_description_content = ParameterValue(Command(['ros2 run xacro xacro ', str(model_path)]), value_type=str)
+ robot_description_content = ParameterValue(Command(['ros2 run xacro xacro ', str(model_path),' sim:=', 'false']), value_type=str)
robot_description = {'robot_description': robot_description_content}
# Nodes
diff --git a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml
index 108b925b..1c0e4c4c 100644
--- a/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml
+++ b/uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_hw/package.xml
@@ -11,6 +11,8 @@
Orson Marmon
Melvin Wang
+ Nafis Molla
+
ament_cmake