From 96d6b0ef7fbc9fb775d66faf6e648bf6c779dda1 Mon Sep 17 00:00:00 2001
From: Michael Ripperger <michael.ripperger@swri.org>
Date: Thu, 15 Aug 2024 18:42:24 -0500
Subject: [PATCH] Updated to SNP 5.0

---
 .github/workflows/ubuntu.yml |  2 +-
 docker/docker-compose.yml    |  2 +-
 launch/start.launch.xml      | 36 +++++++++++++++++++++++++-----------
 package.xml                  |  3 +++
 4 files changed, 30 insertions(+), 13 deletions(-)

diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml
index bbbd0df..c4d518c 100644
--- a/.github/workflows/ubuntu.yml
+++ b/.github/workflows/ubuntu.yml
@@ -21,7 +21,7 @@ jobs:
       matrix:
         distro: [humble]
     container:
-      image: ghcr.io/ros-industrial-consortium/scan_n_plan_workshop:${{ matrix.distro }}-4.4
+      image: ghcr.io/ros-industrial-consortium/scan_n_plan_workshop:${{ matrix.distro }}-5.0
       env:
         CCACHE_DIR: ${{ github.workspace }}/${{ matrix.distro }}/.ccache
         DEBIAN_FRONTEND: noninteractive
diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml
index 5c28eb8..e1f430d 100644
--- a/docker/docker-compose.yml
+++ b/docker/docker-compose.yml
@@ -5,7 +5,7 @@ services:
       context: ..
       dockerfile: docker/Dockerfile
       args:
-        - TAG=humble-4.4
+        - TAG=humble-5.0
     environment:
       DISPLAY: $DISPLAY
       XAUTHORITY: $XAUTHORITY
diff --git a/launch/start.launch.xml b/launch/start.launch.xml
index e3943fc..5131a90 100644
--- a/launch/start.launch.xml
+++ b/launch/start.launch.xml
@@ -32,28 +32,42 @@
 
   <node pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share snp_blending)/config/app.rviz" output="screen">
     <param name="bt_files" value="[$(find-pkg-share snp_application)/config/snp.xml]"/>
+    <param name="bt_plugin_libs" value="[$(find-pkg-prefix snp_application)/lib/libsnp_application_bt_plugins.so]"/>
+    <param name="bt_ros_plugin_libs" value="[$(find-pkg-prefix snp_application)/lib/libsnp_application_bt_plugins.so]"/>
     <param name="tree" value="main"/>
     <!-- Application -->
     <param name="config_file" value="$(find-pkg-share snp_blending)/config/tpp.yaml"/>
     <param name="mesh_file" value="/tmp/results_mesh.ply"/>
     <param name="motion_group" value="manipulator"/>
+    <param name="freespace_motion_group" value="manipulator"/>
     <param name="reference_frame" value="$(var reference_frame)"/>
     <param name="tcp_frame" value="buffy_tcp"/>
     <param name="camera_frame" value="camera_color_optical_frame"/>
     <param name="robot_description" value="$(var robot_description)"/>
     <param name="robot_description_semantic" value="$(var robot_description_semantic)"/>
     <param name="follow_joint_trajectory_action" value="$(var follow_joint_trajectory_action)"/>
-    <!-- TSDF parameters -->
-    <param name="tsdf.min.x" value="0.0"/>
-    <param name="tsdf.min.y" value="0.0"/>
-    <param name="tsdf.min.z" value="0.02"/>
-    <param name="tsdf.max.x" value="0.9144"/>
-    <param name="tsdf.max.y" value="1.524"/>
-    <param name="tsdf.max.z" value="0.5"/>
-    <param name="tsdf.voxel_length" value="0.003"/>
-    <param name="tsdf.sdf_trunc" value="0.03"/>
-    <param name="rgbd.depth_scale" value="1000.0"/>
-    <param name="rgbd.depth_trunc" value="1.1"/>
+    <!-- Home -->
+    <param name="home_state_joint_values" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]"/>
+    <param name="home_state_joint_names" value="['joint_1_s', 'joint_2_l', 'joint_3_u', 'joint_4_r', 'joint_5_b', 'joint_6_t']"/>
+    <param name="freespace_tree" value="go_home_main"/>
+    <!-- Industrial Reconstruction parameters -->
+    <param name="ir.tsdf.min.x" value="0.0"/>
+    <param name="ir.tsdf.min.y" value="0.0"/>
+    <param name="ir.tsdf.min.z" value="0.02"/>
+    <param name="ir.tsdf.max.x" value="0.9144"/>
+    <param name="ir.tsdf.max.y" value="1.524"/>
+    <param name="ir.tsdf.max.z" value="0.5"/>
+    <param name="ir.tsdf.voxel_length" value="0.003"/>
+    <param name="ir.tsdf.sdf_trunc" value="0.03"/>
+    <param name="ir.rgbd.depth_scale" value="1000.0"/>
+    <param name="ir.rgbd.depth_trunc" value="1.1"/>
+    <param name="ir.live" value="true"/>
+    <param name="ir.min_faces" value="3000"/>
+    <param name="ir.normal_angle_tol" value="85.0"/>
+    <param name="ir.normal_x" value="0.0"/>
+    <param name="ir.normal_y" value="0.0"/>
+    <param name="ir.normal_z" value="1.0"/>
+    <param name="ir.archive_dir" value=""/>
   </node>
 
   <!-- Launch Open3d Interface Node -->
diff --git a/package.xml b/package.xml
index 51fe54c..0640397 100644
--- a/package.xml
+++ b/package.xml
@@ -13,6 +13,9 @@
 
   <exec_depend>snp_tpp</exec_depend>
   <exec_depend>snp_application</exec_depend>
+  <exec_depend>snp_scanning</exec_depend>
+  <exec_depend>snp_motion_planning</exec_depend>
+  <exec_depend>snp_motion_execution</exec_depend>
   <exec_depend>industrial_reconstruction</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>