Skip to content

Commit 2cd0381

Browse files
efernandezefernandez
committed
Add diffbot
1 parent bb25c35 commit 2cd0381

File tree

6 files changed

+210
-0
lines changed

6 files changed

+210
-0
lines changed

‎fuse_models/CMakeLists.txt‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -227,6 +227,22 @@ if(CATKIN_ENABLE_TESTING)
227227
CXX_STANDARD_REQUIRED YES
228228
)
229229

230+
# Imu2D tests
231+
add_rostest_gtest(
232+
test_imu_2d
233+
test/imu_2d.test
234+
test/test_imu_2d.cpp
235+
)
236+
target_link_libraries(test_imu_2d
237+
${PROJECT_NAME}
238+
${catkin_LIBRARIES}
239+
)
240+
set_target_properties(test_imu_2d
241+
PROPERTIES
242+
CXX_STANDARD 14
243+
CXX_STANDARD_REQUIRED YES
244+
)
245+
230246
# Unicycle2D Ignition tests
231247
add_rostest_gtest(
232248
test_unicycle_2d_ignition

‎fuse_models/test/diffbot.launch‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
<launch>
2+
<arg name="gui" default="false"/>
3+
4+
<!-- Start world -->
5+
<include file="$(find gazebo_ros)/launch/empty_world.launch">
6+
<arg name="gui" value="$(arg gui)"/>
7+
</include>
8+
9+
<!-- Load diffbot model -->
10+
<param name="robot_description"
11+
command="$(find xacro)/xacro '$(find fuse_models)/test/diffbot.xacro'"/>
12+
13+
<!-- Load diffbot config, e.g. PID gains -->
14+
<rosparam command="load" file="$(find fuse_models)/test/diffbot.yaml"/>
15+
16+
<!-- Spawn diffbot robot in gazebo -->
17+
<node name="spawn_diffbot" pkg="gazebo_ros" type="spawn_model"
18+
args="-urdf -param robot_description -model diffbot -z 0.5"/>
19+
20+
<!-- Load controller config -->
21+
<rosparam command="load" file="$(find fuse_models)/test/diffbot_controllers.yaml"/>
22+
23+
<!-- Spawn controller -->
24+
<node name="controller_spawner" pkg="controller_manager" type="spawner"
25+
args="diffbot_controller"/>
26+
</launch>

‎fuse_models/test/diffbot.xacro‎

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
<?xml version="1.0"?>
2+
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
3+
<xacro:include filename="$(find fuse_models)/test/wheel.xacro"/>
4+
5+
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
6+
7+
<!-- Constants for robot dimensions -->
8+
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
9+
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
10+
<xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
11+
12+
<!-- Links: inertial,visual,collision -->
13+
<link name="base_link">
14+
<inertial>
15+
<!-- origin is relative -->
16+
<origin xyz="0 0 0" rpy="0 0 0"/>
17+
<mass value="5"/>
18+
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
19+
</inertial>
20+
<visual>
21+
<geometry>
22+
<box size="${width} 0.1 0.1"/>
23+
</geometry>
24+
</visual>
25+
<collision>
26+
<!-- origin is relative -->
27+
<origin xyz="0 0 0"/>
28+
<geometry>
29+
<box size="${width} 0.1 0.1"/>
30+
</geometry>
31+
</collision>
32+
</link>
33+
34+
<link name="base_footprint">
35+
<visual>
36+
<geometry>
37+
<sphere radius="0.01"/>
38+
</geometry>
39+
</visual>
40+
<collision>
41+
<origin xyz="0 0 0"/>
42+
<geometry>
43+
<sphere radius="0.00000001"/>
44+
</geometry>
45+
</collision>
46+
</link>
47+
<joint name="base_footprint_joint" type="fixed">
48+
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
49+
<child link="base_link"/>
50+
<parent link="base_footprint"/>
51+
</joint>
52+
53+
<!-- Wheels -->
54+
<xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
55+
<origin xyz="${(width+thickness)/2} 0 0" rpy="0 ${90.0 * deg_to_rad} 0"/>
56+
</xacro:wheel>
57+
<xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
58+
<origin xyz="${-(width+thickness)/2} 0 0" rpy="0 ${90.0 * deg_to_rad} 0"/>
59+
</xacro:wheel>
60+
61+
<!-- IMU -->
62+
<link name="imu_link"/>
63+
<joint name="imu_joint" type="fixed">
64+
<parent link="base_link" />
65+
<child link="imu_link" />
66+
<origin xyz="0.1 -0.2 0.3" rpy="0.0 ${-90.0 * deg_to_rad} ${90.0 * deg_to_rad}"/>
67+
</joint>
68+
69+
<gazebo reference="imu_link">
70+
<gravity>true</gravity>
71+
<sensor name="imu_sensor" type="imu">
72+
<always_on>true</always_on>
73+
<update_rate>50.0</update_rate>
74+
<visualize>true</visualize>
75+
<plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
76+
<topicName>imu</topicName>
77+
<bodyName>imu_link</bodyName>
78+
<updateRateHZ>50.0</updateRateHZ>
79+
<frameName>imu_link</frameName>
80+
<gaussianNoise>0.001</gaussianNoise>
81+
<xyzOffset>0 0 0</xyzOffset>
82+
<rpyOffset>0 0 0</rpyOffset>
83+
<initialOrientationAsReference>false</initialOrientationAsReference>
84+
</plugin>
85+
</sensor>
86+
</gazebo>
87+
88+
<!-- ros_control plugin -->
89+
<gazebo>
90+
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
91+
<robotNamespace>/</robotNamespace>
92+
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
93+
</plugin>
94+
</gazebo>
95+
96+
<!-- Colour -->
97+
<gazebo reference="base_link">
98+
<material>Gazebo/Green</material>
99+
</gazebo>
100+
101+
<gazebo reference="base_footprint">
102+
<material>Gazebo/Purple</material>
103+
</gazebo>
104+
</robot>

‎fuse_models/test/diffbot.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
gazebo_ros_control:
2+
pid_gains:
3+
wheel_0_joint:
4+
p: &p 100.0
5+
wheel_1_joint:
6+
p: *p

‎fuse_models/test/diffbot_controllers.yaml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
diffbot_controller:
2+
type: "diff_drive_controller/DiffDriveController"
3+
left_wheel: 'wheel_0_joint'
4+
right_wheel: 'wheel_1_joint'
5+
publish_rate: 50.0 # defaults to 50
6+
initial_pose_covariance_diagonal: [0.001, 0.001, 0.01]
7+
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests
8+
k_l: 0.01
9+
k_r: 0.01
10+
wheel_resolution: 0.001

‎fuse_models/test/wheel.xacro‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
<?xml version="1.0"?>
2+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3+
<xacro:macro name="wheel" params="name parent radius thickness *origin">
4+
<link name="${name}_link">
5+
<inertial>
6+
<origin xyz="0 0 0" rpy="0 0 0"/>
7+
<mass value="1"/>
8+
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
9+
</inertial>
10+
<visual>
11+
<origin xyz="0 0 0" rpy="0 0 0"/>
12+
<geometry>
13+
<cylinder radius="${wheel_radius}" length="${thickness}"/>
14+
</geometry>
15+
</visual>
16+
<collision>
17+
<origin xyz="0 0 0" rpy="0 0 0"/>
18+
<geometry>
19+
<cylinder radius="${wheel_radius}" length="${thickness}"/>
20+
</geometry>
21+
</collision>
22+
</link>
23+
24+
<joint name="${name}_joint" type="continuous">
25+
<parent link="${parent}_link"/>
26+
<child link="${name}_link"/>
27+
<xacro:insert_block name="origin"/>
28+
<axis xyz="0 0 1"/>
29+
</joint>
30+
31+
<!-- Transmission is important to link the joints and the controller -->
32+
<transmission name="${name}_joint_trans">
33+
<type>transmission_interface/SimpleTransmission</type>
34+
<joint name="${name}_joint">
35+
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
36+
</joint>
37+
<actuator name="${name}_joint_motor">
38+
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
39+
<mechanicalReduction>1</mechanicalReduction>
40+
<motorTorqueConstant>1</motorTorqueConstant>
41+
</actuator>
42+
</transmission>
43+
44+
<gazebo reference="${name}_link">
45+
<material>Gazebo/Red</material>
46+
</gazebo>
47+
</xacro:macro>
48+
</robot>

0 commit comments

Comments
 (0)