From 79276af66f23c2959471928c61fc1ba770c53d69 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Wed, 24 Nov 2021 17:03:14 +0100
Subject: [PATCH] Add hand-eye calibration

---
 launch/env.launch   |  4 +---
 launch/panda.launch | 25 +++++--------------------
 2 files changed, 6 insertions(+), 23 deletions(-)

diff --git a/launch/env.launch b/launch/env.launch
index f4a9eea..4517933 100644
--- a/launch/env.launch
+++ b/launch/env.launch
@@ -6,11 +6,9 @@
   <!-- Load parameters -->
   <rosparam command="load" file="$(find active_grasp)/cfg/active_grasp.yaml" subst_value="true" />
 
-  <!-- Load robot description -->
-  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
-
   <!-- Simulated environment -->
   <group if="$(arg simulation)">
+    <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
     <node pkg="active_grasp" type="bt_sim_node.py" name="bt_sim" output="screen" />
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
   </group>
diff --git a/launch/panda.launch b/launch/panda.launch
index 623e7c2..b2e1536 100644
--- a/launch/panda.launch
+++ b/launch/panda.launch
@@ -3,30 +3,12 @@
   <arg name="robot_ip" default="172.16.0.2" />
   <arg name="load_gripper" default="true" />
 
-  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
-
   <!-- Panda control nodes -->
-  <node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
-    <rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
-    <param name="robot_ip" value="$(arg robot_ip)" />
-  </node>
-
-  <include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
+  <include file="$(find franka_control)/launch/franka_control.launch">
     <arg name="robot_ip" value="$(arg robot_ip)" />
+    <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
-  <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
-  <rosparam command="load" file="$(find panda_controllers)/config/controllers.yaml" />
-
-  <!-- State publisher -->
-  <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
-
-  <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
-    <rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
-    <param name="rate" value="30"/>
-  </node>
-
   <!-- Controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--stopped position_joint_trajectory_controller cartesian_velocity_controller"/>
 
@@ -36,4 +18,7 @@
     <arg name="publish_tf" value="false" />
   </include>
 
+  <!-- Hand-eye calibration -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="to_camera" args="0.108, -0.019, 0.052 0.000, 0.000, 0.720, 0.694 panda_hand camera_depth_optical_frame" />
+
 </launch>