From 4a7fe9071ef26ae1f8926e4ce83a01e2209e995a Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Mon, 22 Jan 2024 12:08:04 +0100
Subject: [PATCH] Added full_example.

---
 examples/README.md           |   5 +-
 examples/box.dae             | 101 +++++++++++++++++++++++++++++++++++
 examples/full_example.launch |  20 +++++++
 examples/full_example.urdf   |  81 ++++++++++++++++++++++++++++
 examples/full_example.yaml   |  42 +++++++++++++++
 rviz/debug.rviz              |   1 -
 6 files changed, 248 insertions(+), 2 deletions(-)
 create mode 100644 examples/box.dae
 create mode 100644 examples/full_example.launch
 create mode 100644 examples/full_example.urdf
 create mode 100644 examples/full_example.yaml

diff --git a/examples/README.md b/examples/README.md
index 9a6ba4d..043725d 100644
--- a/examples/README.md
+++ b/examples/README.md
@@ -1,4 +1,7 @@
 # robot_body_filter example configurations
 
 This folder contains examples of configuration of the filter. 
-Have a look at them to get an idea what should a config look like.
\ No newline at end of file
+Have a look at them to get an idea what should a config look like.
+
+Running `full_example.launch`, you get a quick standalone test of the filter
+that runs everything needed to get a filtered laser scan.
diff --git a/examples/box.dae b/examples/box.dae
new file mode 100644
index 0000000..e83b4ce
--- /dev/null
+++ b/examples/box.dae
@@ -0,0 +1,101 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.83.2</authoring_tool>
+    </contributor>
+    <created>2020-03-09T10:49:51</created>
+    <modified>2020-03-09T10:49:51</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_effects>
+    <effect id="Material-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <color sid="diffuse">0.8 0.8 0.8 1</color>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images/>
+  <library_materials>
+    <material id="Material-material" name="Material">
+      <instance_effect url="#Material-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="Cube-mesh" name="Cube">
+      <mesh>
+        <source id="Cube-mesh-positions">
+          <float_array id="Cube-mesh-positions-array" count="24">1 1 1 1 1 -1 1 -1 1 1 -1 -1 -1 1 1 -1 1 -1 -1 -1 1 -1 -1 -1</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-normals">
+          <float_array id="Cube-mesh-normals-array" count="18">0 0 1 0 -1 0 -1 0 0 0 0 -1 1 0 0 0 1 0</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-normals-array" count="6" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="Cube-mesh-map-0">
+          <float_array id="Cube-mesh-map-0-array" count="72">0.875 0.5 0.625 0.75 0.625 0.5 0.625 0.75 0.375 1 0.375 0.75 0.625 0 0.375 0.25 0.375 0 0.375 0.5 0.125 0.75 0.125 0.5 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.25 0.375 0.5 0.375 0.25 0.875 0.5 0.875 0.75 0.625 0.75 0.625 0.75 0.625 1 0.375 1 0.625 0 0.625 0.25 0.375 0.25 0.375 0.5 0.375 0.75 0.125 0.75 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.25 0.625 0.5 0.375 0.5</float_array>
+          <technique_common>
+            <accessor source="#Cube-mesh-map-0-array" count="36" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="Cube-mesh-vertices">
+          <input semantic="POSITION" source="#Cube-mesh-positions"/>
+        </vertices>
+        <triangles material="Material-material" count="12">
+          <input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#Cube-mesh-map-0" offset="2" set="0"/>
+          <p>4 0 0 2 0 1 0 0 2 2 1 3 7 1 4 3 1 5 6 2 6 5 2 7 7 2 8 1 3 9 7 3 10 5 3 11 0 4 12 3 4 13 1 4 14 4 5 15 1 5 16 5 5 17 4 0 18 6 0 19 2 0 20 2 1 21 6 1 22 7 1 23 6 2 24 4 2 25 5 2 26 1 3 27 3 3 28 7 3 29 0 4 30 2 4 31 3 4 32 4 5 33 0 5 34 1 5 35</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Cube" name="Cube" type="NODE">
+        <matrix sid="transform">0.5 0 0 0 0 1 0 0 0 0 1.5 0 0 0 0 1</matrix>
+        <instance_geometry url="#Cube-mesh" name="Cube">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="Material-material" target="#Material-material">
+                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>
\ No newline at end of file
diff --git a/examples/full_example.launch b/examples/full_example.launch
new file mode 100644
index 0000000..41b94c2
--- /dev/null
+++ b/examples/full_example.launch
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<!-- This is a full example that runs the LaserScan filter, spawns a robot from URDF, creates TF for it and publishes a pointcloud. -->
+<launch>
+    <!-- Prerequisite: sensor_filters, rviz, robot_state_publisher, tf2_ros -->
+    <param name="use_sim_time" value="false" />
+    <param name="robot_description" textfile="$(dirname)/full_example.urdf" />
+    <node name="laser_j_pub" pkg="rostopic" type="rostopic" args="pub -s --use-rostime -r10 joint_states sensor_msgs/JointState &quot;{header: {seq: 0, stamp: now, frame_id: 'base_link'}, name: ['laser_j'], position: [0], velocity: [0], effort: [0]}&quot;" />
+    <node name="state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+    <node name="odom" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 odom base_link" />
+    <!-- http://wiki.ros.org/sensor_filters -->
+    <node name="laser_filter" pkg="sensor_filters" type="laserscan_filter_chain" output="screen">
+        <rosparam command="load" file="$(dirname)/full_example.yaml" />
+        <remap from="~input" to="scan" />
+        <remap from="~output" to="scan_filtered" />
+    </node>
+    <node name="scan" pkg="rostopic" type="rostopic" args="pub -r10 --use-rostime -s /scan sensor_msgs/LaserScan &quot;{header: {seq: 0, stamp: now, frame_id: 'laser'}, angle_min: -0.5, angle_max: 0.5, angle_increment: 0.25, time_increment: 0.1, scan_time: 0.5, range_min: 0.0, range_max: 50.0, ranges: [1, 2, 3, 4, 5], intensities: [1, 1, 1, 1, 1]}&quot;" />
+    <node name="$(anon rviz)" pkg="rviz" type="rviz" args="-d $(dirname)/../rviz/debug.rviz -f base_link">
+        <remap from="nifti_robot_description" to="robot_description" />
+    </node>
+</launch>
diff --git a/examples/full_example.urdf b/examples/full_example.urdf
new file mode 100644
index 0000000..bfcaf73
--- /dev/null
+++ b/examples/full_example.urdf
@@ -0,0 +1,81 @@
+<?xml version="1.0" ?>
+<robot name="NIFTi">
+    <link name="base_link">
+        <visual>
+            <origin rpy="0 0 0" xyz="-0.1220 0 0"/>
+            <geometry><box size="1.8 1.8 1.8"/></geometry>
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="-0.1220 0 0"/>
+            <geometry><box size="1.8 1.8 1.8"/></geometry>
+        </collision>
+        <collision name="big_collision_box">
+            <origin rpy="0 0 0" xyz="-0.1 0 0"/>
+            <!--geometry><box size="2.2545 2.2545 2.2545"/></geometry-->
+            <geometry><mesh filename="package://robot_body_filter/examples/box.dae" scale="2.2545 1.12545 0.7515"/></geometry>
+        </collision>
+        <inertial>
+            <origin rpy="0 0 0" xyz="-0.034 0 0.142"/>
+            <mass value="6.0"/>
+            <inertia ixx="0.001" ixy="0.0001" ixz="0.0" iyy="0.02" iyz="-0.0001" izz="0.03"/>
+        </inertial>
+    </link>
+    <link name="antenna">
+        <visual>
+            <origin rpy="0 0 0" xyz="-0.01864 0 0"/>
+            <geometry><sphere radius="1.24091" /></geometry>
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="-0.01864 0 0"/>
+            <geometry><sphere radius="1.24091"/></geometry>
+        </collision>
+        <inertial>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <mass value="0.5"/>
+            <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.001" iyz="-0.0001" izz="0.004"/>
+        </inertial>
+    </link>
+    <joint name="antenna_j" type="fixed">
+        <parent link="base_link"/>
+        <child link="antenna"/>
+        <origin rpy="0 0 0" xyz="0.01864 0 0"/>
+    </joint>
+    <link name="laser_base">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry><box size="0.0005 0.0005 0.0005"/></geometry>
+        </visual>
+        <inertial>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <mass value="1e-5"/>
+            <inertia ixx="1e-3" ixy="1e-6" ixz="1e-6" iyy="1e-3" iyz="1e-6" izz="1e-3"/>
+        </inertial>
+    </link>
+    <joint name="laser_base_j" type="fixed">
+        <parent link="base_link"/>
+        <child link="laser_base"/>
+        <origin rpy="0 0 0" xyz="-1.5 0 0"/>
+    </joint>
+    <link name="laser">
+        <visual>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry><box size="0.07273 0.07273 0.07273"/></geometry>
+        </visual>
+        <collision>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <geometry><box size="0.07273 0.07273 0.07273"/></geometry>
+        </collision>
+        <inertial>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <mass value="1"/>
+            <inertia ixx="0.003" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.002"/>
+        </inertial>
+    </link>
+    <joint name="laser_j" type="revolute">
+        <parent link="laser_base"/>
+        <child link="laser"/>
+        <axis xyz="1 0 0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <limit effort="0" lower="-2.3561945" upper="2.3561945" velocity="4"/>
+    </joint>
+</robot>
diff --git a/examples/full_example.yaml b/examples/full_example.yaml
new file mode 100644
index 0000000..e1673e3
--- /dev/null
+++ b/examples/full_example.yaml
@@ -0,0 +1,42 @@
+# This is an example filter config for tracked vehicle Absolem from Czech Technical University's
+# VRAS team. The robot is equipped with a 2D lidar attached to a rotating pivot point.
+scan_filter_chain:
+  - name: RobotBodyFilter
+    type: robot_body_filter/RobotBodyFilterLaserScan
+    params:
+      frames/fixed: 'odom'
+      frames/sensor: 'laser'
+      filter/model_pose_update_interval: 0.002  # do not get lower, the processing is then too slow
+      sensor/point_by_point: True
+      sensor/min_distance: 0.03
+      sensor/max_distance: 50.0
+      ignored_links/bounding_sphere: ["antenna", "base_link::big_collision_box"]
+      ignored_links/shadow_test: ["laser", "base_link::big_collision_box"]
+      body_model/inflation/scale: 1.07
+      body_model/inflation/padding: 0.01
+      body_model/robot_description_param: 'robot_description'
+      transforms/buffer_length: 60.0
+      transforms/timeout/reachable: 0.2
+      transforms/timeout/unreachable: 0.2
+      bounding_sphere/compute: True
+      bounding_sphere/debug: True
+      bounding_sphere/marker: True
+      bounding_sphere/publish_cut_out_pointcloud: True
+      bounding_box/compute: True
+      bounding_box/debug: True
+      bounding_box/marker: True
+      bounding_box/publish_cut_out_pointcloud: True
+      oriented_bounding_box/compute: True
+      oriented_bounding_box/debug: True
+      oriented_bounding_box/marker: True
+      oriented_bounding_box/publish_cut_out_pointcloud: True
+      local_bounding_box/frame_id: "base_link"
+      local_bounding_box/compute: True
+      local_bounding_box/debug: True
+      local_bounding_box/marker: True
+      local_bounding_box/publish_cut_out_pointcloud: True
+      debug/pcl/inside: True
+      debug/pcl/clip: True
+      debug/pcl/shadow: True
+      debug/marker/contains: True
+      debug/marker/shadow: True
diff --git a/rviz/debug.rviz b/rviz/debug.rviz
index 2a5f20f..7541eba 100644
--- a/rviz/debug.rviz
+++ b/rviz/debug.rviz
@@ -535,7 +535,6 @@ Visualization Manager:
     - Class: rviz/PublishPoint
       Single click: true
       Topic: /clicked_point
-    - Class: path_planner_rviz_wp_plugin/WaypointsTool
   Value: true
   Views:
     Current: