From fe272257eab5620488bad5391f7484c9c5008479 Mon Sep 17 00:00:00 2001
From: Mo Norman <LTaoist6@gmail.com>
Date: Fri, 20 Jun 2014 10:10:16 +0800
Subject: [PATCH] add loadRobotGraspData in simple_pick_place

---
 clam_pick_place/config/clam_grasp_data.yaml | 11 +++++++++++
 clam_pick_place/launch/pick_place.launch    | 11 +++++++++++
 clam_pick_place/src/simple_pick_place.cpp   |  6 +++++-
 3 files changed, 27 insertions(+), 1 deletion(-)
 create mode 100644 clam_pick_place/config/clam_grasp_data.yaml
 create mode 100644 clam_pick_place/launch/pick_place.launch

diff --git a/clam_pick_place/config/clam_grasp_data.yaml b/clam_pick_place/config/clam_grasp_data.yaml
new file mode 100644
index 0000000..f1782a5
--- /dev/null
+++ b/clam_pick_place/config/clam_grasp_data.yaml
@@ -0,0 +1,11 @@
+base_link: 'base_link'
+
+gripper_group:
+  end_effector_name: 'gripper_group'
+  joints : ['gripper_finger_joint']
+  pregrasp_posture : [0.0]
+  grasp_posture : [1.0]
+  end_effector_parent_link : 'gripper_roll_link'
+  pregrasp_time_from_start : 4.0
+  grasp_time_from_start : 4.0
+  grasp_pose_to_eef : [0,0,0]
diff --git a/clam_pick_place/launch/pick_place.launch b/clam_pick_place/launch/pick_place.launch
new file mode 100644
index 0000000..e96d863
--- /dev/null
+++ b/clam_pick_place/launch/pick_place.launch
@@ -0,0 +1,11 @@
+<launch>
+
+  <arg name="debug" default="true"/>
+  <rosparam command="load" file="$(find clam_pick_place)/config/clam_grasp_data.yaml"/>
+
+  <group>
+    <node name="$(anon simple_pick_place)" type="simple_pick_place" pkg="clam_pick_place">
+    </node>   
+  </group>
+
+</launch>
diff --git a/clam_pick_place/src/simple_pick_place.cpp b/clam_pick_place/src/simple_pick_place.cpp
index b1034d6..8351722 100644
--- a/clam_pick_place/src/simple_pick_place.cpp
+++ b/clam_pick_place/src/simple_pick_place.cpp
@@ -269,8 +269,12 @@ int main(int argc, char **argv)
   // ---------------------------------------------------------------------------------------------
   // Load the Robot Viz Tools for publishing to Rviz
   rviz_tools_.reset(new moveit_visual_tools::VisualTools( BASE_LINK, RVIZ_MARKER_TOPIC));
-                                                              
 
+  if(!grasp_data_.loadRobotGraspData(nh, EE_GROUP))
+  {
+      ROS_ERROR_STREAM_NAMED("simple_pick_place", "Cannot load end_effector data");
+      return 1;
+  }
   // ---------------------------------------------------------------------------------------------
   // Load grasp generator
   moveit_simple_grasps_.reset(new moveit_simple_grasps::SimpleGrasps(rviz_tools_));