Skip to content

Commit

Permalink
cherry picked commit into sim 2024
Browse files Browse the repository at this point in the history
  • Loading branch information
@NafisMolla @andrew2002zhao
NafisMolla authored and andrew2002zhao committed Oct 12, 2023
1 parent d917392 commit 8450971
Show file tree
Hide file tree
Showing 11 changed files with 298 additions and 62 deletions.
3 changes: 3 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
devel/
bin/
build/
install/
log/
msg_gen/
srv_gen/
msg/*Action.msg
Expand Down
128 changes: 93 additions & 35 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
94 changes: 94 additions & 0 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/launch/gazebo2.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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
])
18 changes: 18 additions & 0 deletions uwrt_mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,24 @@
<exec_depend>xacro</exec_depend>
<exec_depend>sensor_msgs</exec_depend>


<exec_depend>controller_manager</exec_depend>
<exec_depend>diff_drive_controller</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>


<exec_depend>ament_index_python</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>diff_drive_controller</exec_depend>



<exec_depend>gazebo_ros2_control</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>ros2_control</exec_depend>
<exec_depend>ros2_controllers</exec_depend>

<test_depend>ament_cmake_flake8</test_depend>
<test_depend>ament_cmake_xmllint</test_depend>

Expand Down
30 changes: 22 additions & 8 deletions ..._mars_rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.macro.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@
<xacro:insert_block name="identical_blocks"/>
</collision>
</xacro:macro>

<!-- macro for the wheel -->
<xacro:macro name="wheel" params="prefix suffix parent_link reflect wheel_diameter:=^ wheel_width:=^ wheel_mass:=^ wheelbase_length:=^ x_position" >
<joint name="${prefix}_${suffix}_wheel_joint" type="continuous">
<axis xyz="0 1 0" rpy="0 0 0" />
Expand All @@ -91,6 +91,20 @@
</link>
</xacro:macro>


<!-- drivetrain macro only for the simulation stuff -->
<xacro:macro name="drivetrain_actuator_gazebo" params="prefix suffix">
<joint name="${prefix}_${suffix}_wheel_joint">
<command_interface name="velocity">
<param name="min">${-max_speed}</param>
<param name="max">${max_speed}</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</xacro:macro>

<!-- actual hardware -->
<xacro:macro name="drivetrain_actuator" params="prefix suffix can_id">
<!-- convert the can_id argument to an xacro property -->
<xacro:property name="can_id_prop" value="${can_id}"/>
Expand All @@ -110,13 +124,13 @@
<!-- <state_interface name="iq_current"/> -->
<!-- TODO: acceleration, torque? -->
</joint>
<transmission name="transmission1">
<!-- TODO: These are not actually parsed by ros2control yet: https://github.com/ros-controls/ros2_control/issues/32, https://github.com/ros-controls/ros2_control/issues/599 -->
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="${prefix}_${suffix}_wheel_joint" role="joint1">
<mechanical_reduction>${mechanical_reduction}</mechanical_reduction>
</joint>
</transmission>
<!-- <transmission name="transmission1"> -->
<!-- TODO: These are not actually parsed by ros2control yet: https://github.com/ros-controls/ros2_control/issues/32, https://github.com/ros-controls/ros2_control/issues/599 -->
<!-- <plugin>transmission_interface/SimpleTransmission</plugin> -->
<!-- <joint name="${prefix}_${suffix}_wheel_joint" role="joint1"> -->
<!-- <mechanical_reduction>${mechanical_reduction}</mechanical_reduction> -->
<!-- </joint> -->
<!-- </transmission> -->
</ros2_control>
</xacro:macro>

Expand Down
18 changes: 9 additions & 9 deletions ...rover_drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,13 @@

<!-- import macros file -->
<xacro:include filename="$(find uwrt_mars_rover_drivetrain_description)/urdf/drivetrain.macro.xacro"/>

<xacro:drivetrain_actuator prefix="right" suffix="front" can_id="5"/>
<xacro:drivetrain_actuator prefix="right" suffix="middle" can_id="4"/>
<xacro:drivetrain_actuator prefix="right" suffix="back" can_id="3"/>
<xacro:drivetrain_actuator prefix="left" suffix="front" can_id="2"/>
<xacro:drivetrain_actuator prefix="left" suffix="middle" can_id="1"/>
<xacro:drivetrain_actuator prefix="left" suffix="back" can_id="0"/>

<xacro:macro name="uwrt_drivetrain_ros2_controls">

<xacro:drivetrain_actuator prefix="right" suffix="front" can_id="5"/>
<xacro:drivetrain_actuator prefix="right" suffix="middle" can_id="4"/>
<xacro:drivetrain_actuator prefix="right" suffix="back" can_id="3"/>
<xacro:drivetrain_actuator prefix="left" suffix="front" can_id="2"/>
<xacro:drivetrain_actuator prefix="left" suffix="middle" can_id="1"/>
<xacro:drivetrain_actuator prefix="left" suffix="back" can_id="0"/>
</xacro:macro>
</robot>

34 changes: 34 additions & 0 deletions ...drivetrain/uwrt_mars_rover_drivetrain_description/urdf/drivetrain.ros2controlgazebo.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<!-- import macros file -->
<xacro:include filename="$(find uwrt_mars_rover_drivetrain_description)/urdf/drivetrain.macro.xacro"/>

<xacro:macro name="uwrt_drivetrain_ros2_controls_gazebo">
<ros2_control name="gazebo_wheel_actuator" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>

<xacro:drivetrain_actuator_gazebo prefix="right" suffix="front" />
<xacro:drivetrain_actuator_gazebo prefix="right" suffix="middle" />
<xacro:drivetrain_actuator_gazebo prefix="right" suffix="back" />
<xacro:drivetrain_actuator_gazebo prefix="left" suffix="front" />
<xacro:drivetrain_actuator_gazebo prefix="left" suffix="middle" />
<xacro:drivetrain_actuator_gazebo prefix="left" suffix="back" />
<!-- -->
</ros2_control>
<!-- controller manager -->
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find uwrt_mars_rover_drivetrain_hw)/config/drivetrain_controllers.yaml</parameters>
<robot_param_node>/robot_state_publisher </robot_param_node>
</plugin>
</gazebo>


</xacro:macro>


</robot>

Loading

0 comments on commit 8450971

Please sign in to comment.