Skip to content

Commit

Permalink
Add controller_config_path arg
Browse files Browse the repository at this point in the history
  • Loading branch information
@rafal-gorecki
rafal-gorecki committed May 15, 2024
1 parent 7a1388e commit f765568
Show file tree
Hide file tree
Showing 5 changed files with 143 additions and 80 deletions.
20 changes: 11 additions & 9 deletions rosbot_xl_controller/launch/controller.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,6 +183,14 @@ def generate_launch_description():
]
)

controller_config_path = PathJoinSubstitution(
[
FindPackageShare("rosbot_xl_controller"),
"config",
controller_config_name,
]
)

robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
Expand All @@ -194,6 +202,8 @@ def generate_launch_description():
"rosbot_xl.urdf.xacro",
]
),
" controller_config_file:=",
controller_config_path,
" mecanum:=",
mecanum,
" lidar_model:=",
Expand All @@ -212,20 +222,12 @@ def generate_launch_description():
)
robot_description = {"robot_description": robot_description_content}

robot_controllers = PathJoinSubstitution(
[
FindPackageShare("rosbot_xl_controller"),
"config",
controller_config_name,
]
)

control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
robot_description,
robot_controllers,
controller_config_path,
],
remappings=[
("imu_sensor_node/imu", "/_imu/data_raw"),
Expand Down
85 changes: 85 additions & 0 deletions rosbot_xl_description/urdf/gazebo.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">

<xacro:macro name="controller" params="config_file namespace:=''">

<xacro:if value="${namespace == 'None'}">
<xacro:property name="namespace" value="" />
</xacro:if>

<gazebo>
<plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">

<parameters>${config_file}</parameters>

<ros>
<namespace>${namespace}</namespace>
<remapping>rosbot_xl_base_controller/cmd_vel_unstamped:=cmd_vel</remapping>
<remapping>/tf:=tf</remapping>
</ros>
</plugin>
</gazebo>
</xacro:macro>

<!-- Gauss noise macro -->
<xacro:macro name="gauss_noise"
params="mean:=0.0 stddev:=0.0 bias_mean:=0.0 bias_stddev:=0.0 precision:=0.0">
<noise type="gaussian">
<mean>${mean}</mean>
<stddev>${stddev}</stddev>
<bias_mean>${bias_mean}</bias_mean>
<bias_stddev>${bias_stddev}</bias_stddev>
<precision>${precision}</precision>
</noise>
</xacro:macro>

<!-- IMU specification: https://dl.btc.pl/kamami_wa/bst_bno055_ds000_12.pdf -->
<xacro:macro name="imu" params="reference_frame namespace:=''">

<xacro:if value="${namespace == 'None'}">
<xacro:property name="ns" value="" />
</xacro:if>
<xacro:unless value="${namespace == 'None'}">
<xacro:property name="ns" value="${namespace}/" />
</xacro:unless>

<gazebo reference="${reference_frame}">
<sensor name="${ns}imu" type="imu">
<always_on>true</always_on>
<update_rate>25</update_rate>
<topic>${ns}imu/data_raw</topic>
<visualize>false</visualize>
<enable_metrics>false</enable_metrics>
<frame_id>imu_link</frame_id>
<ignition_frame_id>imu_link</ignition_frame_id>
<imu>
<angular_velocity>
<!-- rad/s -->
<x>
<xacro:gauss_noise stddev="5.24e-3" bias_mean="1.75e-2" precision="1.07e-3" />
</x>
<y>
<xacro:gauss_noise stddev="5.24e-3" bias_mean="1.75e-2" precision="1.07e-3" />
</y>
<z>
<xacro:gauss_noise stddev="5.24e-3" bias_mean="1.75e-2" precision="1.07e-3" />
</z>
</angular_velocity>
<linear_acceleration>
<!-- m/s^2 | no bias_mean in datasheet -->
<x>
<xacro:gauss_noise stddev="1.86e-3" bias_mean="1.0e-3" precision="2.40e-3" />
</x>
<y>
<xacro:gauss_noise stddev="1.86e-3" bias_mean="1.0e-3" precision="2.40e-3" />
</y>
<z>
<xacro:gauss_noise stddev="1.86e-3" bias_mean="1.0e-3" precision="2.40e-3" />
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
</xacro:macro>

</robot>
41 changes: 17 additions & 24 deletions rosbot_xl_description/urdf/rosbot_xl.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,22 +8,24 @@
<xacro:arg name="use_sim" default="false" />
<xacro:arg name="simulation_engine" default="ignition-gazebo" />
<xacro:arg name="namespace" default="None" />
<xacro:arg name="controller_config_file" default="$(find rosbot_xl_controller)/config/diff_drive_controller.yaml" />

<xacro:include filename="$(find rosbot_xl_description)/urdf/rosbot_xl_macro.urdf.xacro"
ns="husarion" />
<xacro:husarion.rosbot_xl_robot
use_sim="$(arg use_sim)"
controller_config_file="$(arg controller_config_file)"
mecanum="$(arg mecanum)"
namespace="$(arg namespace)"
simulation_engine="$(arg simulation_engine)"
namespace="$(arg namespace)" />
use_sim="$(arg use_sim)" />

<xacro:include filename="$(find rosbot_xl_description)/urdf/components/antenna.urdf.xacro"
ns="antenna" />
<xacro:antenna.antenna
parent_link="body_link"
xyz="-0.155 -0.055 0.065"
rpy="0.0 0.0 0.0"
antenna_angle="0.0" />
parent_link="body_link"
xyz="-0.155 -0.055 0.065"
rpy="0.0 0.0 0.0"
antenna_angle="0.0" />

<!-- INCLUDE LIDAR -->

Expand All @@ -38,8 +40,7 @@
<xacro:lidar.slamtec_rplidar_s1
parent_link="${lidar_parent_link}"
xyz="${lidar_xyz}"
rpy="${lidar_rpy}"
namespace="$(arg namespace)" />
rpy="${lidar_rpy}" />
</xacro:if>

<xacro:if value="${lidar_model == 'slamtec_rplidar_s2'}">
Expand All @@ -48,8 +49,7 @@
<xacro:lidar.slamtec_rplidar_s2
parent_link="${lidar_parent_link}"
xyz="${lidar_xyz}"
rpy="${lidar_rpy}"
namespace="$(arg namespace)" />
rpy="${lidar_rpy}" />
</xacro:if>

<xacro:if value="${lidar_model == 'slamtec_rplidar_s3'}">
Expand All @@ -58,8 +58,7 @@
<xacro:lidar.slamtec_rplidar_s3
parent_link="${lidar_parent_link}"
xyz="${lidar_xyz}"
rpy="${lidar_rpy}"
namespace="$(arg namespace)" />
rpy="${lidar_rpy}" />
</xacro:if>

<xacro:if value="${lidar_model == 'slamtec_rplidar_a2'}">
Expand All @@ -68,8 +67,7 @@
<xacro:lidar.slamtec_rplidar_a2
parent_link="${lidar_parent_link}"
xyz="${lidar_xyz}"
rpy="${lidar_rpy}"
namespace="$(arg namespace)" />
rpy="${lidar_rpy}" />
</xacro:if>

<xacro:if value="${lidar_model == 'slamtec_rplidar_a3'}">
Expand All @@ -78,8 +76,7 @@
<xacro:lidar.slamtec_rplidar_a3
parent_link="${lidar_parent_link}"
xyz="${lidar_xyz}"
rpy="${lidar_rpy}"
namespace="$(arg namespace)" />
rpy="${lidar_rpy}" />
</xacro:if>

<xacro:if value="${lidar_model == 'velodyne_puck'}">
Expand All @@ -88,8 +85,7 @@
<xacro:lidar.velodyne_puck
parent_link="${lidar_parent_link}"
xyz="${lidar_xyz}"
rpy="${lidar_rpy}"
namespace="$(arg namespace)" />
rpy="${lidar_rpy}" />
</xacro:if>

<!-- INCLUDE CAMERA -->
Expand All @@ -109,8 +105,7 @@
parent_link="${camera_parent_link}"
xyz="${camera_xyz}"
rpy="${camera_rpy}"
use_nominal_extrinsics="$(arg use_sim)"
namespace="$(arg namespace)" />
use_nominal_extrinsics="$(arg use_sim)" />
</xacro:if>

<xacro:if value="${camera_model == 'orbbec_astra'}">
Expand All @@ -120,8 +115,7 @@
<xacro:camera.orbbec_astra
parent_link="${camera_parent_link}"
xyz="${camera_xyz}"
rpy="${camera_rpy}"
namespace="$(arg namespace)" />
rpy="${camera_rpy}" />
</xacro:if>

<xacro:if value="${camera_model.startswith('stereolabs')}">
Expand Down Expand Up @@ -151,8 +145,7 @@
parent_link="${camera_parent_link}"
xyz="${camera_xyz}"
rpy="${camera_rpy}"
model="${camera_model_type}"
namespace="$(arg namespace)" />
model="${camera_model_type}" />
</xacro:if>

<!-- INCLUDE CAMERA MOUNT - add if camera_model is not empty -->
Expand Down
58 changes: 11 additions & 47 deletions rosbot_xl_description/urdf/rosbot_xl_macro.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,17 @@
<?xml version='1.0'?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:macro name="rosbot_xl_robot"
params="use_sim mecanum simulation_engine namespace">
params="controller_config_file
mecanum
namespace
simulation_engine
use_sim">

<xacro:if value="${namespace == 'None'}">
<xacro:property name="ns" value="" />
<xacro:property name="gz_control_namespace" value="/" />
</xacro:if>
<xacro:unless value="${namespace == 'None'}">
<xacro:property name="ns" value="${namespace}/" />
<xacro:property name="gz_control_namespace" value="${namespace}" />
<xacro:property name="ns" value="${namespace}/" />
</xacro:unless>

<xacro:if value="${mecanum}">
Expand All @@ -21,6 +23,8 @@

<!-- INCLUDE ROBOT PARTS DEFINITIONS -->
<xacro:include filename="$(find rosbot_xl_description)/urdf/body.urdf.xacro" ns="body" />
<xacro:include filename="$(find rosbot_xl_description)/urdf/gazebo.urdf.xacro" ns="gazebo" />
<xacro:include filename="$(find rosbot_xl_description)/urdf/webots.urdf.xacro" ns="webots" />
<xacro:include filename="$(find rosbot_xl_description)/urdf/wheel.urdf.xacro" ns="wheel" />

<!-- BODY DECLARATION -->
Expand Down Expand Up @@ -126,51 +130,11 @@

<xacro:if value="${use_sim}">
<xacro:if value="${simulation_engine == 'ignition-gazebo'}">
<gazebo>
<plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">

<xacro:if value="${mecanum}">
<parameters>$(find rosbot_xl_controller)/config/mecanum_drive_controller.yaml</parameters>
</xacro:if>
<xacro:unless value="${mecanum}">
<parameters>$(find rosbot_xl_controller)/config/diff_drive_controller.yaml</parameters>
</xacro:unless>

<ros>
<namespace>${gz_control_namespace}</namespace>
<remapping>rosbot_xl_base_controller/cmd_vel_unstamped:=cmd_vel</remapping>
<remapping>/tf:=tf</remapping>
</ros>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<sensor name="${ns}imu" type="imu">
<always_on>true</always_on>
<update_rate>25</update_rate>
<topic>${ns}imu/data_raw</topic>
<visualize>false</visualize>
<enable_metrics>false</enable_metrics>
<frame_id>imu_link</frame_id>
<ignition_frame_id>imu_link</ignition_frame_id>
<ros>
<namespace>${ns}</namespace>
</ros>
</sensor>
</gazebo>
<xacro:gazebo.controller config_file="${controller_config_file}" namespace="${namespace}" />
<xacro:gazebo.imu reference_frame="imu_link" namespace="${namespace}" />
</xacro:if>
<xacro:if value="${simulation_engine == 'webots'}">
<webots>
<plugin type="webots_ros2_control::Ros2Control" />
<plugin type="webots_ros2_driver::Ros2IMU">
<enabled>true</enabled>
<topicName>/imu_broadcaster/imu</topicName>
<alwaysOn>true</alwaysOn>
<frameName>imu_link</frameName>
<gyroName>imu gyro</gyroName>
<accelerometerName>imu accelerometer</accelerometerName>
<inertialUnitName>imu inertial_unit</inertialUnitName>
</plugin>
</webots>
<xacro:webots.controller />
</xacro:if>
</xacro:if>
</xacro:macro>
Expand Down
19 changes: 19 additions & 0 deletions rosbot_xl_description/urdf/webots.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">

<xacro:macro name="controller">
<webots>
<plugin type="webots_ros2_control::Ros2Control" />
<plugin type="webots_ros2_driver::Ros2IMU">
<enabled>true</enabled>
<topicName>/imu_broadcaster/imu</topicName>
<alwaysOn>true</alwaysOn>
<frameName>imu_link</frameName>
<gyroName>imu gyro</gyroName>
<accelerometerName>imu accelerometer</accelerometerName>
<inertialUnitName>imu inertial_unit</inertialUnitName>
</plugin>
</webots>
</xacro:macro>

</robot>

0 comments on commit f765568

Please sign in to comment.