From afa97cfc77313c29b360259ffd7c77f8703f8485 Mon Sep 17 00:00:00 2001
From: Hilario Tome <hilario.tome@pal-robotics.com>
Date: Mon, 31 Oct 2016 20:43:35 +0100
Subject: [PATCH] Not working approach planner

---
 talos_bringup/config/approach_planner_full.yaml | 14 ++++++++++++++
 talos_bringup/launch/play_motion.launch         |  8 +++++---
 talos_bringup/launch/talos_bringup.launch       |  3 +++
 3 files changed, 22 insertions(+), 3 deletions(-)
 create mode 100644 talos_bringup/config/approach_planner_full.yaml

diff --git a/talos_bringup/config/approach_planner_full.yaml b/talos_bringup/config/approach_planner_full.yaml
new file mode 100644
index 0000000..bd16ad4
--- /dev/null
+++ b/talos_bringup/config/approach_planner_full.yaml
@@ -0,0 +1,14 @@
+approach_planner:
+  planning_groups: # Sorted by order of preference
+    - left_arm
+    - right_arm
+    - torso
+    - both_arms_torso
+
+  exclude_from_planning_joints:
+    - head_1_joint
+    - head_2_joint
+
+  joint_tolerance: 0.01
+  skip_planning_approach_vel: 0.5
+  skip_planning_approach_min_dur: 0.5
diff --git a/talos_bringup/launch/play_motion.launch b/talos_bringup/launch/play_motion.launch
index 50c594a..19cc525 100644
--- a/talos_bringup/launch/play_motion.launch
+++ b/talos_bringup/launch/play_motion.launch
@@ -1,12 +1,14 @@
 <?xml version="1.0" encoding="UTF-8"?>
 
 <launch>
-  <arg name="robot" default="full_ft_hey5"/>
 
   <!-- execute pre-recorded motions -->
-  <node pkg="talos_bringup" type="motions_loader.py" name="motions_loader" args="robot=$(arg robot)"/>
+  <rosparam file="$(find talos_bringup)/config/talos_motions.yaml" command="load" />
+  
   <node pkg="play_motion" type="play_motion" name="play_motion">
-    <rosparam file="$(find talos_bringup)/config/approach_planner_$(arg robot).yaml" command="load" />
+    <rosparam file="$(find talos_bringup)/config/approach_planner_full.yaml" command="load" />
   </node>
+  
   <node pkg="play_motion" type="is_already_there.py" name="is_already_there" />
+  
 </launch>
diff --git a/talos_bringup/launch/talos_bringup.launch b/talos_bringup/launch/talos_bringup.launch
index f050b0b..1dad8ea 100644
--- a/talos_bringup/launch/talos_bringup.launch
+++ b/talos_bringup/launch/talos_bringup.launch
@@ -26,4 +26,7 @@
   
   <include file="$(find talos_moveit_config)/launch/move_group.launch" />
   
+    <!-- Pre-recorded motions executor -->
+  <include file="$(find talos_bringup)/launch/play_motion.launch" />
+
 </launch>
-- 
GitLab