From 88766da4caab01fad92a54f323629124dfcf7268 Mon Sep 17 00:00:00 2001
From: engcang <engcang@naver.com>
Date: Fri, 13 Sep 2024 02:54:37 +0900
Subject: [PATCH] locus hilti

---
 .../config/body_filter_params_hilti2021.yaml  |  9 +++++
 .../locus/config/lo_frames_hilti2021.yaml     |  9 +++++
 .../{sim_mid2.launch => hilti2021.launch}     | 22 ++++++-----
 ...nch => robot_description_hilti2021.launch} |  2 +-
 .../config/ntuviral_sensors.yaml              | 39 -------------------
 .../config/sim_sensors.yaml                   | 39 -------------------
 .../urdf/robot_description.urdf.xacro         |  2 +-
 ...=> robot_description_hilti2021.urdf.xacro} | 19 ++++++---
 8 files changed, 45 insertions(+), 96 deletions(-)
 create mode 100644 locus2.0/locus/config/body_filter_params_hilti2021.yaml
 create mode 100644 locus2.0/locus/config/lo_frames_hilti2021.yaml
 rename locus2.0/locus/launch/{sim_mid2.launch => hilti2021.launch} (87%)
 rename locus2.0/locus/launch/{robot_description_sim_mid2.launch => robot_description_hilti2021.launch} (93%)
 delete mode 100644 locus2.0/sensor_description/config/ntuviral_sensors.yaml
 delete mode 100644 locus2.0/sensor_description/config/sim_sensors.yaml
 rename locus2.0/sensor_description/urdf/{robot_description_sim_mid2.urdf.xacro => robot_description_hilti2021.urdf.xacro} (55%)

diff --git a/locus2.0/locus/config/body_filter_params_hilti2021.yaml b/locus2.0/locus/config/body_filter_params_hilti2021.yaml
new file mode 100644
index 0000000..1419698
--- /dev/null
+++ b/locus2.0/locus/config/body_filter_params_hilti2021.yaml
@@ -0,0 +1,9 @@
+# Configure body filter
+body_filter:
+  min_x: -1.0
+  max_x: 1.0
+  min_y: -1.0
+  max_y: 1.0
+  min_z: -1.0
+  max_z: 1.0
+  rotation: 0.0
\ No newline at end of file
diff --git a/locus2.0/locus/config/lo_frames_hilti2021.yaml b/locus2.0/locus/config/lo_frames_hilti2021.yaml
new file mode 100644
index 0000000..61d2083
--- /dev/null
+++ b/locus2.0/locus/config/lo_frames_hilti2021.yaml
@@ -0,0 +1,9 @@
+frame_id:
+  fixed: base_link
+  base: base_link
+  imu: imu_sensor_frame
+  bd_odometry: map
+  artifacts_in_global: true
+
+frame_conversions: 
+  b_convert_imu_to_base_link_frame: true
\ No newline at end of file
diff --git a/locus2.0/locus/launch/sim_mid2.launch b/locus2.0/locus/launch/hilti2021.launch
similarity index 87%
rename from locus2.0/locus/launch/sim_mid2.launch
rename to locus2.0/locus/launch/hilti2021.launch
index d02c674..02f17d5 100644
--- a/locus2.0/locus/launch/sim_mid2.launch
+++ b/locus2.0/locus/launch/hilti2021.launch
@@ -1,7 +1,7 @@
 <launch>
  
-    <arg name="robot_namespace" default="sim"/>
-    <arg name="robot_type" value="sim"/>
+    <arg name="robot_namespace" default="hilti2021"/>
+    <arg name="robot_type" value="hilti2021"/>
 
     <arg name="nodelet_manager" value="nodelet_manager"/>
     <arg name="launch_prefix" value=""/>
@@ -17,23 +17,25 @@
     <arg name="pc_trans_in_1" default="os2/points"/>
 
     <!-- Load robot description -->
-    <include file="$(find locus)/launch/robot_description_sim_mid2.launch">
+    <include file="$(find locus)/launch/robot_description_hilti2021.launch">
         <arg name="robot_namespace" value="$(arg robot_namespace)"/>
     </include>
 
+    <!-- Livox to Ouster: /livox/lidar_ouster -->
+    <node pkg="slict" type="slict_livox_to_ouster" name="slict_livox_to_ouster" respawn="false" output="log" required="true"/>
+
     <group ns="$(arg robot_namespace)">
         <!-- Load parameters -->
-        <rosparam file="$(find locus)/config/body_filter_params_ntuviral.yaml"
+        <rosparam file="$(find locus)/config/body_filter_params_$(arg robot_type).yaml"
                     subst_value="true"/>
 
         <node
             pkg="locus"
             name="locus"
             type="locus_node"
-            output="screen"
-            launch-prefix="gdb -ex run --args">
+            output="screen">           
             <remap from="~LIDAR_TOPIC" to="$(arg pc_input)"/>
-            <remap from="~IMU_TOPIC" to="/mavros/imu/data_raw"/>            
+            <remap from="~IMU_TOPIC" to="/alphasense/imu"/>            
             <remap from="~SPACE_MONITOR_TOPIC" to="localizer_space_monitor/xy_cross_section"/>
 
             <param name="robot_name" value="$(arg robot_namespace)"/>
@@ -42,7 +44,7 @@
             <rosparam file="$(find locus)/config/lo_settings.yaml" />
 
             <param name="b_pub_odom_on_timer" value="false"/>
-            <rosparam file="$(find locus)/config/lo_frames.yaml" subst_value="true"/>
+            <rosparam file="$(find locus)/config/lo_frames_hilti2021.yaml" subst_value="true"/>
             <rosparam file="$(find point_cloud_filter)/config/parameters.yaml"/>
             <rosparam file="$(find point_cloud_odometry)/config/parameters.yaml"/>          
             <rosparam file="$(find point_cloud_localization)/config/ntuviral.yaml"/>
@@ -57,7 +59,7 @@
             type="nodelet"
             name="transform_points_base_link"
             args="standalone pcl/PassThrough">
-            <remap from="~input" to="/livox/pts1"/>
+            <remap from="~input" to="/os_cloud_node/points"/>
             <remap from="~output" to="$(arg pc_trans_in_0)"/>
             <rosparam subst_value="true">
                 filter_field_name: z
@@ -71,7 +73,7 @@
             type="nodelet"
             name="transform_points_base_link_front"
             args="standalone pcl/PassThrough">
-            <remap from="~input" to="/livox/pts2"/>
+            <remap from="~input" to="/livox/lidar_ouster"/>
             <remap from="~output" to="$(arg pc_trans_in_1)"/>
             <rosparam subst_value="true">
                 filter_field_name: z
diff --git a/locus2.0/locus/launch/robot_description_sim_mid2.launch b/locus2.0/locus/launch/robot_description_hilti2021.launch
similarity index 93%
rename from locus2.0/locus/launch/robot_description_sim_mid2.launch
rename to locus2.0/locus/launch/robot_description_hilti2021.launch
index abc1dcc..4270a0d 100644
--- a/locus2.0/locus/launch/robot_description_sim_mid2.launch
+++ b/locus2.0/locus/launch/robot_description_hilti2021.launch
@@ -3,7 +3,7 @@
 
     <param name="robot_description"
            command="$(find xacro)/xacro --inorder
-                    '$(find sensor_description)/urdf/robot_description_sim_mid2.urdf.xacro'
+                    '$(find sensor_description)/urdf/robot_description_hilti2021.urdf.xacro'
                     robot_namespace:=$(arg robot_namespace)"/>
 
     <node pkg="robot_state_publisher"
diff --git a/locus2.0/sensor_description/config/ntuviral_sensors.yaml b/locus2.0/sensor_description/config/ntuviral_sensors.yaml
deleted file mode 100644
index 0f0db81..0000000
--- a/locus2.0/sensor_description/config/ntuviral_sensors.yaml
+++ /dev/null
@@ -1,39 +0,0 @@
-os_sensor1:
-  name: sensor1/os_sensor
-  parent: arch
-  position:
-    x: 0.05
-    y: 0
-    z: 0.055
-  orientation:
-    r: 0
-    p: 0
-    y: 0
-  rate: 10
-  topic_name: /os1_cloud_node1/points
-os_sensor2:
-  name: sensor2/os_sensor
-  parent: arch
-  position:
-    x: -0.55
-    y: 0.03
-    z: 0.09
-  orientation:
-    r: 1.5707963
-    p: 0.0
-    y: 3.1415927
-  rate: 10
-  topic_name: /os1_cloud_node2/points
-imu:
-  name: imu
-  parent: arch
-  topic_name: /imu/imu
-  position:
-    x: 0
-    y: 0
-    z: 0
-  orientation:
-    r: 0
-    p: 0
-    y: 0
-  rate: 400
diff --git a/locus2.0/sensor_description/config/sim_sensors.yaml b/locus2.0/sensor_description/config/sim_sensors.yaml
deleted file mode 100644
index 74c653d..0000000
--- a/locus2.0/sensor_description/config/sim_sensors.yaml
+++ /dev/null
@@ -1,39 +0,0 @@
-livox1:
-  name: sensor1/os_sensor
-  parent: arch
-  position:
-    x: 0.05
-    y: 0
-    z: 0.055
-  orientation:
-    r: 0
-    p: 0
-    y: 0
-  rate: 10
-  topic_name: /os1_cloud_node1/points
-os_sensor2:
-  name: sensor2/os_sensor
-  parent: arch
-  position:
-    x: -0.55
-    y: 0.03
-    z: 0.09
-  orientation:
-    r: 1.5707963
-    p: 0.0
-    y: 3.1415927
-  rate: 10
-  topic_name: /os1_cloud_node2/points
-imu:
-  name: imu
-  parent: arch
-  topic_name: /mavros/imu/data_raw
-  position:
-    x: 0
-    y: 0
-    z: 0
-  orientation:
-    r: 0
-    p: 0
-    y: 0
-  rate: 400
diff --git a/locus2.0/sensor_description/urdf/robot_description.urdf.xacro b/locus2.0/sensor_description/urdf/robot_description.urdf.xacro
index a6bf673..29ea196 100644
--- a/locus2.0/sensor_description/urdf/robot_description.urdf.xacro
+++ b/locus2.0/sensor_description/urdf/robot_description.urdf.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot name="husky_sensors" xmlns:xacro="http://ros.org/wiki/xacro">
-<xacro:property name="sensor_props" value="${load_yaml('$(find sensor_description)/config/$(arg robot_namespace)_sensors.yaml')}" />
+<!-- <xacro:property name="sensor_props" value="${load_yaml('$(find sensor_description)/config/$(arg robot_namespace)_sensors.yaml')}" /> -->
 
   <!-- Base Link -->
   <link name="base_link"/>
diff --git a/locus2.0/sensor_description/urdf/robot_description_sim_mid2.urdf.xacro b/locus2.0/sensor_description/urdf/robot_description_hilti2021.urdf.xacro
similarity index 55%
rename from locus2.0/sensor_description/urdf/robot_description_sim_mid2.urdf.xacro
rename to locus2.0/sensor_description/urdf/robot_description_hilti2021.urdf.xacro
index ba4d72b..7813216 100644
--- a/locus2.0/sensor_description/urdf/robot_description_sim_mid2.urdf.xacro
+++ b/locus2.0/sensor_description/urdf/robot_description_hilti2021.urdf.xacro
@@ -13,18 +13,25 @@
     <origin xyz="0.0 0 0.0" rpy="0 0 0" />
   </joint>
 
-  <link name="livox1"/>
+  <link name="os_sensor"/>
   <joint name="arch_to_sensor1" type="fixed">
     <parent link="arch" />
-    <child link="livox1" />
-    <origin xyz="0.1 0 0.1" rpy="0 2.4958208 0"/>
+    <child link="os_sensor" />
+    <origin xyz="0.0102543784 -0.0066573859 0.130729657" rpy="-3.1412457 0.0065189 0.0024579"/>
   </joint>
 
-  <link name="livox2"/>
+  <link name="livox_frame"/>
   <joint name="arch_to_sensor2" type="fixed">
     <parent link="arch" />
-    <child link="livox2" />
-    <origin xyz="0.0 0.0 -0.1" rpy="0 -2.4958208 0"/>
+    <child link="livox_frame" />
+    <origin xyz="-0.003050707070885951 0.021993853931529066 0.15076415229379997" rpy="-3.1244532 0.0067876 0.0171429"/>
+  </joint>
+
+  <link name="imu_sensor_frame"/>
+  <joint name="arch_to_imu" type="fixed">
+    <parent link="arch" />
+    <child link="imu_sensor_frame" />
+    <origin xyz="0.0 0 0.0" rpy="0 0 0"/>
   </joint>
 
 </robot>