From 2e1f3eb4a7f4ab0ac2dd74979f354b7f3896d033 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 19 Nov 2021 18:04:54 +0100
Subject: [PATCH] Add launch file for panda

---
 launch/panda.launch | 42 ++++++++++++++++++++++++++++--------------
 1 file changed, 28 insertions(+), 14 deletions(-)

diff --git a/launch/panda.launch b/launch/panda.launch
index 1b67c8f..623e7c2 100644
--- a/launch/panda.launch
+++ b/launch/panda.launch
@@ -1,25 +1,39 @@
 <?xml version="1.0" ?>
 <launch>
-  <!-- Parameters -->
-  <arg name="launch_rviz" default="false" />
   <arg name="robot_ip" default="172.16.0.2" />
+  <arg name="load_gripper" default="true" />
 
-  <!-- Transforms -->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="to_world" args="0 0 0 0 0 0 world panda_link0" />
-  <node pkg="tf2_ros" type="static_transform_publisher" name="to_camera" args="0.03226862 -0.06137175  0.04107702 0.00099995 0. 0.39898185 0.91695828 panda_link8 camera_depth_optical_frame" />
+  <param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/franka/panda_arm_hand.urdf.xacro" />
 
-  <!-- Launch camera driver -->
+  <!-- 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)">
+    <arg name="robot_ip" value="$(arg robot_ip)" />
+  </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"/>
+
+  <!-- Camera -->
   <include file="$(find realsense2_camera)/launch/rs_camera.launch">
     <arg name="enable_pointcloud" value="true" />
     <arg name="publish_tf" value="false" />
   </include>
 
-  <!-- Launch panda driver -->
-  <include file="$(find franka_control)/launch/franka_control.launch">
-    <arg name="robot_ip" value="$(arg robot_ip)" />
-    <!-- <arg name="load_robot_description" value="$(arg load_robot_description)" /> -->
-  </include>
-
-  <!-- Launch rviz -->
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/active_grasp.rviz" if="$(arg launch_rviz)" />
 </launch>