Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions src/bringup/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,6 @@
<exec_depend>rclpy</exec_depend>
<exec_depend>controller_manager_msgs</exec_depend>

<exec_depend>arm_bringup</exec_depend>
<exec_depend>drive_bringup</exec_depend>
<exec_depend>science_bringup</exec_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
35 changes: 0 additions & 35 deletions src/description/config/athena_drive_sim_controllers.yaml

This file was deleted.

1 change: 0 additions & 1 deletion src/description/launch/display.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@ def generate_launch_description():
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
robot_description_path,
" use_mock_hardware:=true",
]
)

Expand Down
9 changes: 3 additions & 6 deletions src/description/urdf/athena_arm.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,7 @@

<!-- Launch file arguments -->
<xacro:arg name="prefix" default=""/>
<xacro:arg name="use_mock_hardware" default="false"/>
<xacro:arg name="mock_sensor_commands" default="false"/>
<xacro:arg name="sim_gazebo_classic" default="true"/>
<xacro:arg name="sim_gazebo" default="true"/>
<xacro:arg name="use_sim" default="false"/>
<xacro:arg name="simulation_controllers" default=""/>

<!-- Macro Imports -->
Expand Down Expand Up @@ -40,10 +37,10 @@
<xacro:description/>


<xacro:if value="$(arg use_mock_hardware)"> <!-- MOCK -->
<xacro:if value="$(arg use_sim)">
<xacro:mock_ros2_control name="mock"/>
</xacro:if>
<xacro:unless value="$(arg use_mock_hardware)"> <!-- HARDWARE -->
<xacro:unless value="$(arg use_sim)">
<xacro:smc_ros2_control name="smc"/>
<xacro:talon_ros2_control name="talon"/>
<xacro:rmd_ros2_control name="rmd"/>
Expand Down
13 changes: 5 additions & 8 deletions src/description/urdf/athena_drive.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,8 @@
<!-- Use this if parameters are set from the launch file, otherwise delete -->
<xacro:arg name="prefix" default=""/>

<xacro:arg name="use_mock_hardware" default="false"/>
<xacro:arg name="mock_sensor_commands" default="false"/>
<xacro:arg name="sim_gazebo" default="true"/>

<xacro:arg name="use_sim" default="false"/>
<xacro:arg name="simulation_controllers" default=""/>


Expand Down Expand Up @@ -37,16 +36,14 @@
<!-- Contains description of drive -->
<xacro:description/>

<xacro:if value="$(arg use_mock_hardware)">
<xacro:if value="$(arg use_sim)">
<xacro:athena_drive_ros2_control
name="athena_drive"
prefix="$(arg prefix)"
use_mock_hardware="$(arg use_mock_hardware)"
mock_sensor_commands="$(arg mock_sensor_commands)"
sim_gazebo="$(arg sim_gazebo)"
use_sim="true"
simulation_controllers="$(arg simulation_controllers)"/>
</xacro:if>
<xacro:unless value="$(arg use_mock_hardware)">
<xacro:unless value="$(arg use_sim)">
<xacro:odrive_ros2_control name="odrive"/>
<xacro:rmd_ros2_control name="rmd"/>
</xacro:unless>
Expand Down
243 changes: 107 additions & 136 deletions src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro
Original file line number Diff line number Diff line change
@@ -1,141 +1,112 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">

<xacro:macro name="athena_drive_ros2_control" params="
<xacro:macro name="athena_drive_ros2_control" params="
name
prefix
use_mock_hardware:=false
mock_sensor_commands:=false
sim_gazebo:=false
simulation_controllers"
>

<ros2_control name="${name}" type="system">
<hardware>
<xacro:if value="${sim_gazebo}">
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</xacro:if>

<xacro:if value="${use_mock_hardware and not sim_gazebo}">
<plugin>mock_components/GenericSystem</plugin>
<param name="mock_sensor_commands">${mock_sensor_commands}</param>
</xacro:if>

<xacro:unless value="${use_mock_hardware or sim_gazebo}">
<plugin>mock_components/GenericSystem</plugin>
</xacro:unless>
</hardware>

<joint name="${prefix}suspension_front_left_joint">
<command_interface name="position">
<param name="min">-1.0471975512</param>
<param name="max">1.0471975512</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}suspension_front_right_joint">
<command_interface name="position">
<param name="min">-1.0471975512</param>
<param name="max">1.0471975512</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}steer_fl_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.2832</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}steer_fr_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.2832</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}steer_br_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.28318530718</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}steer_bl_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.2832</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>


<joint name="${prefix}propulsion_fl_joint">
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}propulsion_fr_joint">
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>


<joint name="${prefix}propulsion_br_joint">
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>


<joint name="${prefix}propulsion_bl_joint">
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>

<xacro:if value="$(arg sim_gazebo)">
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<xacro:if value="${simulation_controllers != ''}">
<parameters>${simulation_controllers}</parameters>
</xacro:if>
<controller_manager_node_name>${prefix}controller_manager</controller_manager_node_name>
</plugin>
</gazebo>
</xacro:if>

</xacro:macro>
prefix use_sim:=false
simulation_controllers">

<ros2_control name="${name}" type="system">
<hardware>
<xacro:if value="$(arg use_sim)">
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</xacro:if>

<xacro:unless value="$(arg use_sim)">
<plugin>mock_components/GenericSystem</plugin>
</xacro:unless>
</hardware>

<joint name="${prefix}steer_fl_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.2832</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}steer_fr_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.2832</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}steer_br_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.28318530718</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}steer_bl_joint">
<command_interface name="position">
<param name="min">0</param>
<param name="max">6.2832</param>
</command_interface>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>


<joint name="${prefix}propulsion_fl_joint">
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

<joint name="${prefix}propulsion_fr_joint">
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>


<joint name="${prefix}propulsion_br_joint">
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>


<joint name="${prefix}propulsion_bl_joint">
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>

<xacro:if value="$(arg use_sim)">
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<xacro:if value="${simulation_controllers != ''}">
<parameters>${simulation_controllers}</parameters>
</xacro:if>
<controller_manager_node_name>${prefix}controller_manager</controller_manager_node_name>
</plugin>
</gazebo>
</xacro:if>

</xacro:macro>
</robot>
6 changes: 3 additions & 3 deletions src/description/urdf/athena_drive/athena_drive_macro.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,7 @@
<child link="${prefix}base_link"/>
</joint>
<!-- joint 1 -->
<joint name="${prefix}suspension_left_joint" type="revolute">
<joint name="${prefix}suspension_front_left_joint" type="revolute">
<origin xyz="0 0 0.3" rpy="${pi} 0 0"/>
<parent link="${prefix}base_link"/>
<child link="${prefix}link1"/>
Expand Down Expand Up @@ -217,7 +217,7 @@
<limit effort="0" lower="${radians(-120)}" upper="${radians(120)}" velocity="${radians(540)}"/>
</joint>
<!-- joint 6 -->
<joint name="${prefix}suspension_right_joint" type="revolute">
<joint name="${prefix}suspension_front_right_joint" type="revolute">
<origin xyz="0.125 0 0" rpy="0 ${-pi/2} 0"/>
<parent link="${prefix}link5"/>
<child link="${prefix}link6"/>
Expand All @@ -226,7 +226,7 @@
</joint>

<!-- tool frame - fixed frame -->
<joint name="${prefix}suspension_right_joint-tool0" type="fixed">
<joint name="${prefix}suspension_front_right_joint-tool0" type="fixed">
<parent link="${prefix}link6"/>
<child link="${prefix}tool0"/>
<origin xyz="0 0 -0.115" rpy="0 ${-pi} ${-pi/2}"/>
Expand Down
Loading