Skip to content

Commit

Permalink
Support printing pose elements in degrees, added tests
Browse files Browse the repository at this point in the history 
Signed-off-by: Aaron Chong <[email protected]>
  • Loading branch information
@aaronchongth
aaronchongth committed Aug 31, 2021
1 parent 6f148fa commit 40f2575
Show file tree
Hide file tree
Showing 6 changed files with 171 additions and 72 deletions.
6 changes: 5 additions & 1 deletion src/Param.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -301,7 +301,11 @@ std::string Param::GetAsString() const
{
StringStreamClassicLocale ss;

ss << ParamStreamer{ this->dataPtr->value };
if (this->dataPtr->strValue.has_value())
ss << this->dataPtr->strValue.value();
else
ss << ParamStreamer{ this->dataPtr->value };

return ss.str();
}

Expand Down
32 changes: 16 additions & 16 deletions test/integration/include_custom_model_expected_output.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,9 @@
</box>
</geometry>
<material>
<ambient>0.5 0.5 1 1</ambient>
<diffuse>0.5 0.5 1 1</diffuse>
<specular>0 0 1 1</specular>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'>
Expand Down Expand Up @@ -72,12 +72,12 @@
<resolution>0.01</resolution>
</range>
</lidar>
<visualize>1</visualize>
<visualize>true</visualize>
<alwaysOn>1</alwaysOn>
</sensor> -->

<sensor name='camera' type='camera'>
<pose>-1.06 0 0 0 -0 3.14</pose>
<pose>-1.06 0 0 0 0 3.14</pose>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
Expand All @@ -92,15 +92,15 @@

<!-- removed -->
<!-- <always_on>1</always_on>
<visualize>1</visualize> -->
<visualize>true</visualize> -->

<update_rate>30</update_rate>
<topic>chassis/camera</topic>
</sensor>

<!-- added -->
<visual name='camera_visual'>
<pose>-1.06 0 0 0 -0 3.14</pose>
<pose>-1.06 0 0 0 0 3.14</pose>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
Expand All @@ -110,7 +110,7 @@
</link>

<link name='top'>
<pose>0.6 0 0.7 0 -0 0</pose>
<pose>0.6 0 0.7 0 0 0</pose>

<collision name='top_collision'>
<geometry>
Expand Down Expand Up @@ -170,7 +170,7 @@
</collision> -->

<visual name='camera_visual'>
<pose>-0.2 0 0.3 0 -0 3.14</pose>
<pose>-0.2 0 0.3 0 0 3.14</pose>

<geometry>
<sphere>
Expand Down Expand Up @@ -205,13 +205,13 @@
</visual>

<sensor name='cam' type='camera' custom:test='hello'>
<pose relative_to='__model__'>0.1 0.2 0.3 0 -0 0</pose>
<pose relative_to='__model__'>0.1 0.2 0.3 0 0 0</pose>
<camera name='top_camera'>
<!-- modified with above -->
<!-- <sensor name='camera' type='camera'>
<pose>-0.2 0 0.3 0 -0 0</pose>
<camera> -->
<horizontal_fov>1.047</horizontal_fov>
<horizontal_fov>1.0469999999999999</horizontal_fov>
<image>
<width>1280</width>
<!-- modified with above -->
Expand All @@ -232,7 +232,7 @@
<update_rate>20</update_rate>
<!-- modified with above -->
<!-- <update_rate>30</update_rate> -->
<visualize>1</visualize>
<visualize>true</visualize>
<topic>top/cam</topic>
<!-- modified with above -->
<!-- <topic>top/camera</topic> -->
Expand Down Expand Up @@ -332,7 +332,7 @@

</link>
<link name='caster'>
<pose relative_to='right_wheel'>0 0 0 0 0 0</pose>
<pose relative_to='right_wheel'>0 0 0 0 -0 0</pose>
<!-- modified with above -->
<!-- <pose relative_to='__model__'>-0.957138 -0 -0.125 0 -0 0</pose> -->
<inertial>
Expand Down Expand Up @@ -408,7 +408,7 @@
</joint>

<model name='nested_models'>
<pose relative_to='caster'>1 1 1 0 -0 0</pose>
<pose relative_to='caster'>1 1 1 0 0 0</pose>
<!-- modified with above -->
<!-- <pose relative_to='__model__'>0 0 0 0 -0 0</pose> -->
<model name='model1'>
Expand All @@ -429,7 +429,7 @@

<!-- added -->
<link name='new_link'>
<pose>0 0 0 0 -0 0</pose>
<pose>0 0 0 0 0 0</pose>
<sensor name='camera_sensor' type='camera'>
<camera>
<horizontal_fov>1.047</horizontal_fov>
Expand All @@ -444,7 +444,7 @@
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>1</visualize>
<visualize>true</visualize>
</sensor>
</link>

Expand Down
76 changes: 38 additions & 38 deletions test/integration/include_custom_nested_model_expected_output.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
<ixx>0.126164</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.41651899999999997</iyy>
<iyy>0.416519</iyy>
<iyz>0</iyz>
<izz>0.481014</izz>
</inertia>
Expand All @@ -23,9 +23,9 @@
</box>
</geometry>
<material>
<ambient>0.5 0.5 1 1</ambient>
<diffuse>0.5 0.5 1 1</diffuse>
<specular>0 0 1 1</specular>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'>
Expand Down Expand Up @@ -73,32 +73,32 @@
<resolution>0.01</resolution>
</range>
</lidar>
<visualize>1</visualize>
<visualize>true</visualize>
<alwaysOn>1</alwaysOn>
</sensor> -->

<sensor name='camera' type='camera'>
<pose>-1.06 0 0 0 -0 3.14</pose>
<pose>-1.06 0 0 0 0 3.14</pose>
<camera>
<horizontal_fov>1.0469999999999999</horizontal_fov>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.10000000000000001</near>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>1</visualize>
<visualize>true</visualize>
<topic>chassis/camera</topic>
</sensor>

<!-- added -->
<visual name="camera_visual">
<pose>-1.06 0 0 0 -0 3.14</pose>
<pose>-1.06 0 0 0 0 3.14</pose>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
Expand All @@ -108,7 +108,7 @@
</link>
<model name='top_model'>
<link name='top'>
<pose>0.6 0 0.7 0 -0 0</pose>
<pose>0.6 0 0.7 0 0 0</pose>
<collision name='top_collision'>
<geometry>
<box>
Expand All @@ -132,7 +132,7 @@
<model name='top_camera'>
<link name='top_camera_link'>
<visual name='camera_visual'>
<pose>-0.2 0 0.3 0 -0 3.14</pose>
<pose>-0.2 0 0.3 0 0 3.14</pose>
<geometry>
<sphere>
<radius>0.05</radius>
Expand All @@ -154,39 +154,39 @@
</material>
</visual>
<sensor name='camera' type='camera'>
<pose>0 0 0 0 -0 3.14</pose>
<pose>0 0 0 0 0 3.14</pose>
<!-- modified with above -->
<!-- <pose>-0.2 0 0.3 0 -0 0</pose> -->
<camera>
<horizontal_fov>1.0469999999999999</horizontal_fov>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.10000000000000001</near>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>60</update_rate>
<!-- modified with above -->
<!-- <update_rate>30</update_rate> -->
<visualize>1</visualize>
<visualize>true</visualize>
<topic>top/camera</topic>
</sensor>

<!-- added -->
<collision name='camera_collision'>
<pose>-0.2 0 0.3 0 -0 3.14</pose>
<pose>-0.2 0 0.3 0 0 3.14</pose>
<geometry>
<sphere>
<radius>0.050000000000000003</radius>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
</link>
<pose relative_to='top'>0 0 0 0 -0 0</pose>
<pose relative_to='top'>0 0 0 0 0 0</pose>
</model>
<joint name='top_to_camera' type='fixed'>
<parent>top</parent>
Expand All @@ -199,22 +199,22 @@
</joint>
<model name='left_wheel'>
<link name='wheel_link'>
<pose>0 0 0 0 -0 0</pose>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.14583299999999999</ixx>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.14583299999999999</iyy>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.29999999999999999</radius>
<radius>0.3</radius>
</sphere>
</geometry>
<material>
Expand All @@ -226,7 +226,7 @@
<collision name='collision'>
<geometry>
<sphere>
<radius>0.29999999999999999</radius>
<radius>0.3</radius>
</sphere>
</geometry>
</collision>
Expand All @@ -235,22 +235,22 @@
</model>
<model name='right_wheel'>
<link name='wheel_link'>
<pose>0 0 0 0 -0 0</pose>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.14583299999999999</ixx>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.14583299999999999</iyy>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.29999999999999999</radius>
<radius>0.3</radius>
</sphere>
</geometry>
<material>
Expand All @@ -262,7 +262,7 @@
<collision name='collision'>
<geometry>
<sphere>
<radius>0.29999999999999999</radius>
<radius>0.3</radius>
</sphere>
</geometry>
</collision>
Expand All @@ -271,22 +271,22 @@
</model>
<model name='caster'>
<link name='wheel_link'>
<pose>0 0 0 0 -0 0</pose>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.14583299999999999</ixx>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.14583299999999999</iyy>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.29999999999999999</radius>
<radius>0.3</radius>
</sphere>
</geometry>
<material>
Expand All @@ -298,7 +298,7 @@
<collision name='collision'>
<geometry>
<sphere>
<radius>0.29999999999999999</radius>
<radius>0.3</radius>
</sphere>
</geometry>
</collision>
Expand All @@ -311,8 +311,8 @@
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1.7976900000000001e+308</lower>
<upper>1.7976900000000001e+308</upper>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
</limit>
</axis>
</joint>
Expand All @@ -322,8 +322,8 @@
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1.7976900000000001e+308</lower>
<upper>1.7976900000000001e+308</upper>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
</limit>
</axis>
</joint>
Expand Down
Loading

0 comments on commit 40f2575

Please sign in to comment.