Merge lp:~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte into lp:sr-ros-interface/fuerte

[Toni Oliver]

Created a new cpp library to allow to send targets to both CAN and ethercat hand, with any controllers running. sr_move_arm/hand_posture_execution is now using this library. Adapted reactive_grasp to use the shadowhand_ros.py library instead of publishing directly the commands. This allows to use the ethercat hand without launching the separate compatibility node.

Created a new cpp library to allow to send targets to both CAN and ethercat hand, with any controllers running. sr_move_arm/hand_posture_execution is now using this library. Adapted reactive_grasp to use the shadowhand_ros.py library instead of publishing directly the commands. This allows to use the ethercat hand without launching the separate compatibility node.

https://app.asana.com/0/1470949453683/2390846818451

[markpitchless (markpitchless)]

http://reviewboard.shadow.local/r/111/

[lp:~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte]

396. By markpitchless on 2012-11-12

Rename ShadowhandRos to HandCommander.

397. By markpitchless on 2012-11-12

Start a test. Fails at the moment.

398. By markpitchless on 2012-11-12

Fix the test, we need to be a node.

399. By markpitchless on 2012-11-12

Convert to a test that launches nodes.

400. By markpitchless on 2012-11-12

Use size_t

401. By markpitchless on 2012-11-12

Cleanup for better readability.

402. By markpitchless on 2012-11-12

Send updates after ethercat init.

403. By markpitchless on 2012-11-12

Update sr_arm_move to use renamed shadowrobot::HandCommander

404. By markpitchless on 2012-11-13

Fix and test hand posture action server.

* Fixed bug with joint names on pre and grasp requests.
* Added a basic rostest.

[Ugo (ugocupcic)]

Don't use #define (ever), use static const.

review: Needs Fixing

[Ugo (ugocupcic)]

Do you mind if we name the cpp header files *.hpp ?? This way I have a different mode in emacs for C headers (.h) and C++ headers (.hpp)...

[Ugo (ugocupcic)]

Excellent use of boost::ptr_map!

[Ugo (ugocupcic)]

Why is hand_posture_execution_test in sr_move_arm/bin/test and not in /test? It seems a bit confusing (I usually wipe bin)

[Ugo (ugocupcic)]

> Don't use #define (ever), use static const.

I fixed it.

[Ugo (ugocupcic)]

The test is failing here :S was it working for you??

review: Needs Fixing

[markpitchless (markpitchless)]

> Why is hand_posture_execution_test in sr_move_arm/bin/test and not in /test?
> It seems a bit confusing (I usually wipe bin)

That is where the ros wiki tutorials on rostest tell you to put them. The add exe commands in the cmake file for the test are still relative to bin/

[markpitchless (markpitchless)]

> The test is failing here :S was it working for you??

Damn. Yes, both the tests I added passed. Which test and what the error?

[Ugo (ugocupcic)]

OK thanks for this

On Wed, Nov 14, 2012 at 11:53 AM, markpitchless <email address hidden>wrote:

> > Why is hand_posture_execution_test in sr_move_arm/bin/test and not in
> /test?
> > It seems a bit confusing (I usually wipe bin)
>
> That is where the ros wiki tutorials on rostest tell you to put them. The
> add exe commands in the cmake file for the test are still relative to bin/
> --
>
> https://code.launchpad.net/~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte/+merge/133688
> You are reviewing the proposed merge of
> lp:~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte into
> lp:sr-ros-interface/fuerte.
>

--
Ugo Cupcic | Shadow Robot Company | <email address hidden>
Senior Software Engineer | 251 Liverpool Road |
need a Hand? | London N1 1LX | [phone]
http://www.shadowrobot.com/hand/ @shadowrobot

[markpitchless (markpitchless)]

> Do you mind if we name the cpp header files *.hpp ?? This way I have a
> different mode in emacs for C headers (.h) and C++ headers (.hpp)...

No worries from me.

[Ugo (ugocupcic)]

Here's the error message;
http://pastebin.com/gQZML6t3

On Wed, Nov 14, 2012 at 11:54 AM, markpitchless <email address hidden>wrote:

> > The test is failing here :S was it working for you??
>
> Damn. Yes, both the tests I added passed. Which test and what the error?
> --
>
> https://code.launchpad.net/~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte/+merge/133688
> You are reviewing the proposed merge of
> lp:~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte into
> lp:sr-ros-interface/fuerte.
>

--
Ugo Cupcic | Shadow Robot Company | <email address hidden>
Senior Software Engineer | 251 Liverpool Road |
need a Hand? | London N1 1LX | [phone]
http://www.shadowrobot.com/hand/ @shadowrobot

[lp:~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte]

405. By Toni Oliver on 2012-11-14

Increased the time for the gazebo simulated hand to load from 2 s to 15 s which should be enough even for a slow computer.

[Toni Oliver (toliver-shadow)]

I solved the test failure and pushed, but it's the only thing I did, the rest of modifications (the #define, etc) are still pending.
I increased the time for the gazebo simulated hand to load from 2 s to 15 s which should be enough even for a much slower computer.
We have to admit it Ugo, Mark has a faster computer than me and you.

> Here's the error message;
> http://pastebin.com/gQZML6t3
>
>
> On Wed, Nov 14, 2012 at 11:54 AM, markpitchless
> <email address hidden>wrote:
>
> > > The test is failing here :S was it working for you??
> >
> > Damn. Yes, both the tests I added passed. Which test and what the error?
> > --
> >
> > https://code.launchpad.net/~shadowrobot/sr-ros-
> interface/Feature_ethercat_sr_move_arm_fuerte/+merge/133688
> > You are reviewing the proposed merge of
> > lp:~shadowrobot/sr-ros-interface/Feature_ethercat_sr_move_arm_fuerte into
> > lp:sr-ros-interface/fuerte.
> >
>
>
>
> --
> Ugo Cupcic | Shadow Robot Company | <email address hidden>
> Senior Software Engineer | 251 Liverpool Road |
> need a Hand? | London N1 1LX | [phone]
> http://www.shadowrobot.com/hand/ @shadowrobot

[Ugo (ugocupcic)]

> > Do you mind if we name the cpp header files *.hpp ?? This way I have a
> > different mode in emacs for C headers (.h) and C++ headers (.hpp)...
>
> No worries from me.
fixed

[Ugo (ugocupcic)]

OK everything is now fixed and tested. Good job!

review: Approve

=== modified file '.bzrignore'
--- .bzrignore	2012-10-22 08:48:10 +0000
+++ .bzrignore	2012-11-14 19:52:22 +0000
@@ -308,3 +308,5 @@
 sr_utilities/.pydevproject
 sr_utilities/cmake_install.cmake
 sr_mechanism_controllers/_gtest_from_src
+sr_hand/_gtest_from_src
+sr_move_arm/bin/hand_posture_execution

=== modified file 'sr_hand/CMakeLists.txt'
--- sr_hand/CMakeLists.txt	2012-07-23 14:42:29 +0000
+++ sr_hand/CMakeLists.txt	2012-11-14 19:52:22 +0000
@@ -157,3 +157,15 @@
 #   EXAMPLES  #
 ###############
 rosbuild_add_executable(examples/link_joints examples/link_joints.cpp)
+
+############################################################################
+#   Hand command library (gives compatibility with CAN and ethercat hand)  #
+############################################################################
+rosbuild_add_library(HandCommander src/hand_commander.cpp)
+rosbuild_link_boost(HandCommander thread)
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+#rosbuild_add_gtest(test/hand_commander_test test/hand_commander_test.cpp)
+rosbuild_add_executable(test/hand_commander_test test/hand_commander_test.cpp)
+rosbuild_add_gtest_build_flags(test/hand_commander_test)
+target_link_libraries(test/hand_commander_test HandCommander)
+rosbuild_add_rostest(test/hand_commander_test.launch)

=== added directory 'sr_hand/bin/test'
=== added file 'sr_hand/include/sr_hand/hand_commander.h'
--- sr_hand/include/sr_hand/hand_commander.h	1970-01-01 00:00:00 +0000
+++ sr_hand/include/sr_hand/hand_commander.h	2012-11-14 19:52:22 +0000
@@ -0,0 +1,89 @@
+/**
+ * @file   hand_commander.h
+ * @author Toni Oliver <[e-mail]>, Contact <[e-mail]>
+ * @date   Thu Nov 08 15:34:37 2012
+ *
+*
+* Copyright 2011 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation, either version 2 of the License, or (at your option)
+* any later version.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program.  If not, see <http://www.gnu.org/licenses/>.
+*
+ * @brief  This is a library that offers a simple interface to send commands to hand joints.
+ * It is compatible with the Shadow Robot CAN hand and ethercat hand.
+ * It allows the user not worry about the name of the currently running controllers.
+ * Only position control is allowed (targets must represent angles).
+ *
+ *
+ */
+
+#pragma once
+
+#include <ros/ros.h>
+#include <vector>
+#include <string>
+#include <map>
+
+#include <boost/smart_ptr.hpp>
+#include <boost/ptr_container/ptr_map.hpp>
+#include <sr_robot_msgs/joint.h>
+
+using namespace ros;
+
+
+namespace shadowhandRosLib
+{
+  enum HandType
+  {
+    UNKNOWN,
+    CAN,
+    ETHERCAT
+  };
+}
+
+namespace shadowrobot
+{
+
+/**
+ * This ROS subscriber is used to issue commands to the hand / arm, from sending a set of targets, to changing the
+ * controller parameters.
+ */
+class HandCommander
+{
+public:
+    HandCommander();
+
+    /// Destructor
+    ~HandCommander();
+
+    void sendCommands(std::vector<sr_robot_msgs::joint> joint_vector);
+
+private:
+    ///ros node handle
+    NodeHandle node_;
+    ///Publisher for the CAN hand targets
+    Publisher sr_hand_target_pub;
+    ///Publishers for the ethercat hand targets for every joint
+    boost::ptr_map<std::string,Publisher> sr_hand_target_pub_map;
+
+    shadowhandRosLib::HandType hand_type;
+    bool ethercat_controllers_found;
+
+    /**
+     * init function for the ethercat hand
+     * It can be called if we know that there's an ethercat hand (pr2_controller_manager running)
+     */
+    void initializeEthercatHand();
+}; // end class ShadowhandSubscriber
+
+} // end namespace

=== modified file 'sr_hand/manifest.xml'
--- sr_hand/manifest.xml	2012-10-19 16:34:36 +0000
+++ sr_hand/manifest.xml	2012-11-14 19:52:22 +0000
@@ -19,6 +19,7 @@
   <depend package="sensor_msgs"/>
   <depend package="sr_robot_msgs"/>
   <depend package="pr2_controllers_msgs"/>
+  <depend package="pr2_mechanism_msgs"/>
   <depend package="sr_utilities"/>
   <depend package="diagnostic_msgs"/>
   <depend package="dynamic_reconfigure"/>
@@ -28,7 +29,7 @@
   <depend package="gazebo_msgs"/>
 
   <export>
-	<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lSrVirtual"/>
+	<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lSrVirtual -lHandCommander"/>
 	<python path="${prefix}/python_lib"/>
 	<python path="${prefix}/python_lib/grasp"/>
   </export>

=== added file 'sr_hand/src/hand_commander.cpp'
--- sr_hand/src/hand_commander.cpp	1970-01-01 00:00:00 +0000
+++ sr_hand/src/hand_commander.cpp	2012-11-14 19:52:22 +0000
@@ -0,0 +1,121 @@
+/**
+ * @file   hand_commander.cpp
+ * @author Toni Oliver <[e-mail]>, Contact <[e-mail]>
+ * @date   Thu Nov 08 15:34:37 2012
+ *
+*
+* Copyright 2011 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation, either version 2 of the License, or (at your option)
+* any later version.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program.  If not, see <http://www.gnu.org/licenses/>.
+*
+ * @brief  This is a library that offers a simple interface to send commands to hand joints.
+ * It is compatible with the Shadow Robot CAN hand and ethercat hand.
+ * It allows the user not worry about the name of the currently running controllers.
+ * Only position control is allowed (targets must represent angles).
+ *
+ *
+ */
+
+#include <sr_hand/hand_commander.h>
+#include <pr2_mechanism_msgs/ListControllers.h>
+#include <sr_robot_msgs/sendupdate.h>
+#include <std_msgs/Float64.h>
+
+
+namespace shadowrobot
+{
+
+#define TIMEOUT_TO_DETECT_CONTROLLER_MANAGER 3.0
+
+HandCommander::HandCommander():
+    hand_type(shadowhandRosLib::UNKNOWN),
+    ethercat_controllers_found(false)
+{
+  //We use the presence of the pr2_controller_manager/list_controllers service to detect that the hand is ethercat
+  if(ros::service::waitForService("pr2_controller_manager/list_controllers", ros::Duration(TIMEOUT_TO_DETECT_CONTROLLER_MANAGER)))
+  {
+    hand_type = shadowhandRosLib::ETHERCAT;
+    initializeEthercatHand();
+    ROS_INFO("HandCommander library: ETHERCAT hand detected");
+  }
+  else
+  {
+    hand_type = shadowhandRosLib::CAN;
+    sr_hand_target_pub = node_.advertise<sr_robot_msgs::sendupdate>("/srh/sendupdate", 2);
+    ROS_INFO("HandCommander library: CAN hand detected");
+  }
+}
+
+HandCommander::~HandCommander()
+{
+}
+
+void HandCommander::initializeEthercatHand()
+{
+  sr_hand_target_pub_map.clear();
+
+  ros::ServiceClient controller_list_client = node_.serviceClient<pr2_mechanism_msgs::ListControllers>("pr2_controller_manager/list_controllers");
+
+  pr2_mechanism_msgs::ListControllers controller_list;
+  std::string controlled_joint_name;
+
+  controller_list_client.call(controller_list);
+
+  for (size_t i=0;i<controller_list.response.controllers.size() ;i++ )
+  {
+    if(controller_list.response.state[i]=="running")
+    {
+      std::string controller = controller_list.response.controllers[i];
+      if (node_.getParam("/"+controller+"/joint", controlled_joint_name))
+      {
+        ROS_DEBUG("controller %d:%s controls joint %s\n",
+            (int)i,controller.c_str(),controlled_joint_name.c_str());
+        sr_hand_target_pub_map[controlled_joint_name]
+            = node_.advertise<std_msgs::Float64>(controller+"/command", 2);
+        ethercat_controllers_found = true;
+      }
+    }
+  }
+
+}
+
+void HandCommander::sendCommands(std::vector<sr_robot_msgs::joint> joint_vector)
+{
+  if(hand_type == shadowhandRosLib::ETHERCAT)
+  {
+    if(!ethercat_controllers_found)
+    {
+      initializeEthercatHand();
+      // No controllers we found so bail out
+      if (!ethercat_controllers_found)
+          return;
+    }
+    for(size_t i = 0; i < joint_vector.size(); ++i)
+    {
+      std_msgs::Float64 target;
+      target.data = joint_vector.at(i).joint_target * M_PI/180.0;
+      sr_hand_target_pub_map[joint_vector.at(i).joint_name].publish(target);
+    }
+  }
+  else
+  {
+    sr_robot_msgs::sendupdate sendupdate_msg;
+    sendupdate_msg.sendupdate_length = joint_vector.size();
+    sendupdate_msg.sendupdate_list = joint_vector;
+
+    sr_hand_target_pub.publish(sendupdate_msg);
+  }
+}
+
+}

=== added directory 'sr_hand/test'
=== added file 'sr_hand/test/hand_commander_test.cpp'
--- sr_hand/test/hand_commander_test.cpp	1970-01-01 00:00:00 +0000
+++ sr_hand/test/hand_commander_test.cpp	2012-11-14 19:52:22 +0000
@@ -0,0 +1,25 @@
+
+// We are testing this
+#include "sr_hand/hand_commander.h"
+
+// ROS
+#include "ros/ros.h"
+
+// Gtest
+#include <gtest/gtest.h>
+
+using namespace shadowrobot;
+
+TEST(HandCommander, constructor)
+{
+    HandCommander handcmd = HandCommander();
+    EXPECT_TRUE(true);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    ros::init(argc, argv, "hand_commander_test");
+    ros::NodeHandle nh; // init the node
+    return RUN_ALL_TESTS();
+}

=== added file 'sr_hand/test/hand_commander_test.launch'
--- sr_hand/test/hand_commander_test.launch	1970-01-01 00:00:00 +0000
+++ sr_hand/test/hand_commander_test.launch	2012-11-14 19:52:22 +0000
@@ -0,0 +1,3 @@
+<launch>
+    <test test-name="hand_commander_test" pkg="sr_hand" type="hand_commander_test" />
+</launch>

=== modified file 'sr_move_arm/CMakeLists.txt'
--- sr_move_arm/CMakeLists.txt	2011-03-04 18:29:05 +0000
+++ sr_move_arm/CMakeLists.txt	2012-11-14 19:52:22 +0000
@@ -23,3 +23,5 @@
 
 rosbuild_add_executable(hand_posture_execution src/hand_posture_execution.cpp)
 rosbuild_link_boost(hand_posture_execution thread)
+
+rosbuild_add_rostest(test/hand_posture_execution_test.launch)

=== added directory 'sr_move_arm/bin'
=== added directory 'sr_move_arm/bin/test'
=== added file 'sr_move_arm/bin/test/hand_posture_execution_test'
--- sr_move_arm/bin/test/hand_posture_execution_test	1970-01-01 00:00:00 +0000
+++ sr_move_arm/bin/test/hand_posture_execution_test	2012-11-14 19:52:22 +0000
@@ -0,0 +1,69 @@
+#! /usr/bin/env python
+PKG='sr_move_arm'
+import roslib; roslib.load_manifest(PKG)
+import rospy
+import unittest
+
+import actionlib
+from object_manipulation_msgs.msg import *
+from sensor_msgs.msg import *
+import math
+
+class HandPostureExecutionTest(unittest.TestCase):
+
+    def assertJointPos(self, joint_name, ok_pos):
+        """Test a joints position. Pos is in degrees."""
+        msg = rospy.wait_for_message('/gazebo/joint_states', JointState, timeout = 3.0)
+        pos = round(math.degrees(msg.position[msg.name.index(joint_name)]), 0)
+        self.assertEqual( pos, ok_pos,
+                "Joint %s is not at position %i degrees, got %i"%(joint_name, ok_pos, pos) )
+
+    def test_basic(self):
+        client = actionlib.SimpleActionClient(
+                '/right_arm/hand_posture_execution',GraspHandPostureExecutionAction)
+        client.wait_for_server()
+
+        jnames = ['FFJ4', 'FFJ3', 'FFJ0',
+                'MFJ4', 'MFJ3', 'MFJ0',
+                'RFJ4', 'RFJ3', 'RFJ0',
+                'LFJ5', 'LFJ4', 'LFJ3', 'LFJ0',
+                'THJ5', 'THJ4', 'THJ3', 'THJ2', 'THJ1']
+
+        # We send a pre grasp followed by a grasp to test the 2 flows through
+        # the action server.
+        # TODO: Add a release at the end
+
+        # Zero the hand
+        goal = GraspHandPostureExecutionGoal()
+        goal.goal = GraspHandPostureExecutionGoal.PRE_GRASP
+        goal.grasp.pre_grasp_posture = JointState()
+        goal.grasp.pre_grasp_posture.name = jnames
+        goal.grasp.pre_grasp_posture.position = [0] * len(jnames)
+        client.send_goal(goal)
+        client.wait_for_result(rospy.Duration.from_sec(5.0))
+        # Wait for the move. TODO: Check the action state instead of sleeping
+        rospy.sleep(10.0)
+        self.assertJointPos("THJ4", 0)
+
+        # Move the thumb in
+        target_pos = 70 # deg
+        goal = GraspHandPostureExecutionGoal()
+        goal.goal = GraspHandPostureExecutionGoal.GRASP
+        goal.grasp.grasp_posture = JointState()
+        goal.grasp.grasp_posture.name = jnames
+        goal.grasp.grasp_posture.position = [0] * len(jnames)
+        goal.grasp.grasp_posture.position[jnames.index("THJ4")] = math.radians(target_pos)
+        client.send_goal(goal)
+        client.wait_for_result(rospy.Duration.from_sec(5.0))
+        # Wait for the move. TODO: Check the action state instead of sleeping
+        rospy.sleep(12.0)
+        self.assertJointPos("THJ4", target_pos)
+
+
+
+if __name__ == '__main__':
+    rospy.sleep(15.0) # Dirty little sleep to wait for things to settle
+    import rostest
+    rospy.init_node('hand_posture_execution_test')
+    rostest.rosrun(PKG, 'hand_posture_execution_test', HandPostureExecutionTest)
+

=== modified file 'sr_move_arm/include/sr_move_arm/hand_posture_execution.hpp'
--- sr_move_arm/include/sr_move_arm/hand_posture_execution.hpp	2011-11-11 16:04:37 +0000
+++ sr_move_arm/include/sr_move_arm/hand_posture_execution.hpp	2012-11-14 19:52:22 +0000
@@ -12,8 +12,8 @@
 
 #include <ros/ros.h>
 #include <boost/smart_ptr.hpp>
-#include <sr_robot_msgs/sendupdate.h>
 #include <sr_robot_msgs/is_hand_occupied.h>
+#include <sr_hand/hand_commander.h>
 
 #include <actionlib/server/simple_action_server.h>
 #include <object_manipulation_msgs/GraspHandPostureExecutionAction.h>
@@ -41,10 +41,9 @@
 
     boost::shared_ptr<actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction> > action_server;
 
-    Publisher sr_hand_target_pub;
+    boost::shared_ptr<shadowrobot::HandCommander> shadowhand_ros_lib;
     ServiceServer get_status_server;
     ServiceClient is_hand_occupied_client;
-    sr_robot_msgs::sendupdate sendupdate_msg;
     std::vector<sr_robot_msgs::joint> joint_vector;
 
     bool hand_occupied;
@@ -57,4 +56,4 @@
 End:
 */
 
-#endif SR_MOVE_ARM_SIMPLE_ACTION_H
+#endif //SR_MOVE_ARM_SIMPLE_ACTION_H

=== modified file 'sr_move_arm/manifest.xml'
--- sr_move_arm/manifest.xml	2012-03-16 11:04:42 +0000
+++ sr_move_arm/manifest.xml	2012-11-14 19:52:22 +0000
@@ -14,6 +14,7 @@
   <url>http://ros.org/wiki/sr_move_arm</url>
   <depend package="roscpp"/>
   <depend package="rospy"/>
+  <depend package="rostest"/>
   <depend package="std_msgs"/>
   <depend package="std_srvs"/>
   <depend package="arm_navigation_msgs"/>
@@ -24,6 +25,7 @@
   <depend package="actionlib_msgs"/>
   <depend package="sr_robot_msgs"/>
   <depend package="object_manipulator"/>
+  <depend package="sr_hand"/>
 
 </package>
 

=== modified file 'sr_move_arm/src/hand_posture_execution.cpp'
--- sr_move_arm/src/hand_posture_execution.cpp	2012-07-25 20:52:06 +0000
+++ sr_move_arm/src/hand_posture_execution.cpp	2012-11-14 19:52:22 +0000
@@ -21,7 +21,7 @@
 
     get_status_server = nh.advertiseService("/right_arm/grasp_status", &SrHandPostureExecutionSimpleAction::getStatusCallback, this);
 
-    sr_hand_target_pub = nh.advertise<sr_robot_msgs::sendupdate>("/srh/sendupdate", 2);
+    shadowhand_ros_lib = boost::shared_ptr<shadowrobot::HandCommander>( new shadowrobot::HandCommander() );
 
     action_server->start();
   }
@@ -61,7 +61,18 @@
     }
 
 
-    std::vector<std::string> joint_names = goal->grasp.pre_grasp_posture.name;
+    std::vector<std::string> joint_names;
+    if (goal->goal == object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP)
+      joint_names = goal->grasp.grasp_posture.name;
+    else
+      joint_names = goal->grasp.pre_grasp_posture.name;
+    
+    if (joint_names.empty())
+    {
+      ROS_ERROR("Shadow Robot grasp execution: names empty in requested grasp");
+      action_server->setAborted();
+      return;
+    }
 
     joint_vector.clear();
     for(unsigned int i = 0; i < joint_names.size(); ++i)
@@ -70,7 +81,6 @@
       joint.joint_name = joint_names[i];
       joint_vector.push_back(joint);
     }
-    sendupdate_msg.sendupdate_length = joint_vector.size();
 
     object_manipulation_msgs::GraspHandPostureExecutionResult result;
 
@@ -81,18 +91,17 @@
 
       if (goal->grasp.grasp_posture.position.empty())
       {
-	ROS_ERROR("Shadow Robot grasp execution: position vector empty in requested grasp");
-	action_server->setAborted();
-	return;
+        ROS_ERROR("Shadow Robot grasp execution: position vector empty in requested grasp");
+        action_server->setAborted();
+        return;
       }
       for(unsigned int i = 0; i < goal->grasp.grasp_posture.position.size(); ++i)
       {
         joint_vector[i].joint_target = goal->grasp.grasp_posture.position[i]*180.0/M_PI;
         ROS_DEBUG("[%s]: %f", joint_names[i].c_str(), joint_vector[i].joint_target);
       }
-      sendupdate_msg.sendupdate_list = joint_vector;
 
-      sr_hand_target_pub.publish(sendupdate_msg);
+      shadowhand_ros_lib->sendCommands(joint_vector);
       ROS_DEBUG("Hand in grasp position");
 
       result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
@@ -106,9 +115,9 @@
 
       if (goal->grasp.pre_grasp_posture.position.empty())
       {
-	ROS_ERROR("Shadow Robot grasp execution: position vector empty in requested pre_grasp");
-	action_server->setAborted();
-	return;
+        ROS_ERROR("Shadow Robot grasp execution: position vector empty in requested pre_grasp");
+        action_server->setAborted();
+        return;
       }
       //move to pregrasp
       for(unsigned int i = 0; i < goal->grasp.pre_grasp_posture.position.size(); ++i)
@@ -116,9 +125,8 @@
         joint_vector[i].joint_target = goal->grasp.pre_grasp_posture.position[i]*180.0/M_PI;
         ROS_DEBUG("[%s]: %f", joint_names[i].c_str(), joint_vector[i].joint_target);
       }
-      sendupdate_msg.sendupdate_list = joint_vector;
 
-      sr_hand_target_pub.publish(sendupdate_msg);
+      shadowhand_ros_lib->sendCommands(joint_vector);
       ROS_DEBUG("Hand in pregrasp position");
 
       result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
@@ -136,9 +144,8 @@
         joint_vector[i].joint_target = 0.0;
         ROS_DEBUG("[%s]: %f", joint_names[i].c_str(), joint_vector[i].joint_target);
       }
-      sendupdate_msg.sendupdate_list = joint_vector;
 
-      sr_hand_target_pub.publish(sendupdate_msg);
+      shadowhand_ros_lib->sendCommands(joint_vector);
       ROS_DEBUG("Hand opened");
 
       result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
@@ -178,3 +185,4 @@
    c-basic-offset: 2
 End:
 */
+// vim: expandtab:ts=2:sw=2

=== modified file 'sr_move_arm/src/reactive_actions/reactive_grasp.py'
--- sr_move_arm/src/reactive_actions/reactive_grasp.py	2012-09-28 18:50:23 +0000
+++ sr_move_arm/src/reactive_actions/reactive_grasp.py	2012-11-14 19:52:22 +0000
@@ -8,7 +8,6 @@
 import scipy, time
 from object_manipulator.convert_functions import *
 from object_manipulation_msgs.msg import GripperTranslation, ManipulationPhase
-from sr_robot_msgs.msg import sendupdate, joint
 from sr_robot_msgs.srv import which_fingers_are_touching
 from trajectory_msgs.msg import JointTrajectory
 from actionlib import SimpleActionClient
@@ -17,6 +16,8 @@
 #from geometric_shapes_msgs.msg import Shape
 from actionlib_msgs.msg import GoalStatus
 
+from shadowhand_ros import ShadowHand_ROS
+
 ##abort exception
 class Aborted(Exception): pass
 
@@ -54,8 +55,7 @@
         self._phase_pub = rospy.Publisher('/reactive_manipulation_phase', ManipulationPhase)
 
         #send targets to the arm / hand
-        self.sr_hand_target_pub = rospy.Publisher('/srh/sendupdate', sendupdate)
-        self.sr_arm_target_pub = rospy.Publisher('/sr_arm/sendupdate', sendupdate)
+        self.sr_lib   = ShadowHand_ROS()
         self.move_arm_client = None
 
         if which_arm == 'r':
@@ -235,15 +235,15 @@
             joint_names = joint_path.joint_names
 
             for trajectory_step in joint_path.points:
-                sendupdate_msg = []
+                sendupdate_dict = {}
 
                 for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions):
                     # convert targets to degrees
-                    sendupdate_msg.append(joint(joint_name = joint_name, joint_target = joint_target * 57.325))
-
-                self.sr_arm_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
-                self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
-
+                    sendupdate_dict[joint_name] = joint_target * 57.325
+
+                self.sr_lib.sendupdate_arm_from_dict(sendupdate_dict)
+                self.sr_lib.sendupdate_from_dict(sendupdate_dict)
+                
                 current_sleep_time = trajectory_step.time_from_start - last_time
                 rospy.sleep( current_sleep_time )
                 last_time = trajectory_step.time_from_start
@@ -269,14 +269,14 @@
             joint_names = joint_path.joint_names
 
             for trajectory_step in joint_path.points:
-                sendupdate_msg = []
+                sendupdate_dict = {}
 
                 for (joint_name, joint_target) in zip(joint_names, trajectory_step.positions):
                     # convert targets to degrees
-                    sendupdate_msg.append(joint(joint_name = joint_name, joint_target = joint_target * 57.325))
+                    sendupdate_dict[joint_name] = joint_target * 57.325
 
-                self.sr_arm_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
-                self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
+                self.sr_lib.sendupdate_arm_from_dict(sendupdate_dict)
+                self.sr_lib.sendupdate_from_dict(sendupdate_dict)
 
                 current_sleep_time = trajectory_step.time_from_start - last_time
                 rospy.sleep( current_sleep_time )
@@ -352,7 +352,7 @@
         fingers_touching = [0,0,0,0,0]
 
         for i_step in range(0, nb_steps + 1):
-            sendupdate_msg = []
+            sendupdate_dict = {}
             fingers_touch_index = {"FF":0, "MF":1, "RF":2, "LF":3, "TH":4}
             for (index, joint_name, grasp_target, pregrasp_target) in zip(range(0,len(joint_names)),joint_names, grasp, pregrasp):
                 # only update the finger that are not touching
@@ -368,11 +368,11 @@
                     joint_target = pregrasp_target + float(grasp_target - pregrasp_target)*(float(i_step) / float(nb_steps) )
                     myjoint_target = joint_target * 180.0 / math.pi
                     current_targets[index] = joint_target
-                    sendupdate_msg.append(joint(joint_name = joint_name, joint_target = myjoint_target))
+                    sendupdate_dict[joint_name] = myjoint_target
                     rospy.logdebug("["+joint_name+"]: (p/g/t) = "+str(pregrasp_target)+"/"+str(grasp_target)+"/"+str(myjoint_target) + " ("+
                                    str(float(i_step) / float(nb_steps))+"%)")
 
-            self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
+            self.sr_lib.sendupdate_from_dict(sendupdate_dict)
             rospy.sleep(iteration_time)
             #check which fingers are touching
             which_fingers_are_touching_response = []
@@ -469,13 +469,13 @@
         """
         joint_names = pregrasp_posture.name
         joint_targets = pregrasp_posture.position
-        sendupdate_msg = []
+        sendupdate_dict = {}
 
         for (joint_name, joint_target) in zip(joint_names, joint_targets):
             myjoint_target=joint_target * 180 / math.pi
-            sendupdate_msg.append(joint(joint_name = joint_name, joint_target = myjoint_target))
+            sendupdate_dict[joint_name] = myjoint_target
 
-        self.sr_hand_target_pub.publish(sendupdate(len(sendupdate_msg), sendupdate_msg) )
+        self.sr_lib.sendupdate_from_dict(sendupdate_dict)
 
     def command_cartesian(self, pose, frame_id='world'):
         """

=== added file 'sr_move_arm/test/hand_posture_execution_test.launch'
--- sr_move_arm/test/hand_posture_execution_test.launch	1970-01-01 00:00:00 +0000
+++ sr_move_arm/test/hand_posture_execution_test.launch	2012-11-14 19:52:22 +0000
@@ -0,0 +1,7 @@
+<launch>
+    <!-- Start simulated hand for testing -->
+    <include file="$(find sr_hand)/launch/gazebo/gazebo_hand.launch" />
+
+    <node name="hand_posture_execution" pkg="sr_move_arm" type="hand_posture_execution" />
+    <test test-name="hand_posture_execution_test" pkg="sr_move_arm" type="hand_posture_execution_test" />
+</launch>