updated pick and place demo and grasping test

CMakeLists.txt

@@ -17,6 +17,8 @@ find_package(catkin REQUIRED
         joint_trajectory_controller
         robot_state_publisher
         roscpp
+        tf2
+        tf2_geometry_msgs
         std_msgs
         xacro
         moveit_core
@@ -42,6 +44,7 @@ catkin_package(CATKIN_DEPENDS
         controller_interface
         hardware_interface
         pluginlib
+        tf2
         simulation_util
         DEPENDS
         # system_lib
@@ -52,15 +55,27 @@ catkin_package(CATKIN_DEPENDS
 # ######################################################################################################################
 
 # Specify additional locations of header files Your package locations should be listed before other locations
-include_directories(${catkin_INCLUDE_DIRS})
+include_directories(src ${catkin_INCLUDE_DIRS})
 
 add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)
 add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)
 add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)
-add_executable(PickPlacePlanner src/PickPlacePlanner.cpp)
+add_executable(PickPlacePlanner src/PickPlacePlanner.cpp src/PickPlacePlannerMain.cpp)
 
 ## Add cmake target dependencies of the executable
 target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES})
 target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES})
 target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES})
 target_link_libraries(PickPlacePlanner ${catkin_LIBRARIES})
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+
+if(CATKIN_ENABLE_TESTING)
+    find_package(rostest REQUIRED)
+    add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp src/PickPlacePlanner.cpp)
+    target_link_libraries(tests_grasping ${catkin_LIBRARIES})
+endif()
\ No newline at end of file

package.xml

@@ -42,11 +42,12 @@
   <depend>robot_state_publisher</depend>
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
-  <depend>tf</depend>
   <depend>xacro</depend>
   <depend>moveit_simple_controller_manager</depend>
 
   <!-- CUSTOM -->
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_ros_perception</depend>

src/PickPlacePlanner.cpp

@@ -53,11 +53,15 @@
 #include <moveit/move_group_interface/move_group_interface.h>
 
 // TF2
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 // Zone Spawner to add the collision objects to Gazebo
 #include <GazeboZoneSpawner.h>
 
+#include "PickPlacePlanner.h"
+
+namespace pick_place_planner {
+
 /**
  * Sets the posture argument to an open state for the two gripper fingers (8cm apart).
  * @param posture An empty posture that is filled with a posture for two fingers
@@ -100,7 +104,7 @@ void closedGripper(trajectory_msgs::JointTrajectory& posture)
  * Pick an object from a fixed position.
  * @param move_group The move group the pick operation is performed on
  */
-void pick(moveit::planning_interface::MoveGroupInterface& move_group)
+moveit::planning_interface::MoveItErrorCode pick(moveit::planning_interface::MoveGroupInterface& move_group)
 {
   // Create a vector of grasps to be attempted, currently only creating single grasp.
   // This is essentially useful when using a grasp generator to generate and test multiple grasps.
@@ -114,8 +118,46 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group)
   // your manipulator which in this case would be `"panda_link8"` You will have to compensate for the
   // transform from `"panda_link8"` to the palm of the end effector.
   grasps[0].grasp_pose.header.frame_id = "panda_link0";
-  tf2::Quaternion orientation;
-  orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
+
+  // object orientation (default orientation)
+  tf2::Quaternion object_orientation{0, 0, 0, 1};
+
+  // gripper orientation
+  tf2::Quaternion gripper_orientation;
+  // We know the gripper is rotated -45 degrees from the urdf model:
+  // The "hand" definition in franka_description/robots/hand.xacro
+  //     <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
+  //     <xacro:macro name="hand" params="connected_to:='' ns:='' rpy:='0 0 0' xyz:='0 0 0' ">
+  //       <xacro:unless value="${connected_to == ''}">
+  //         <joint name="${ns}_hand_joint" type="fixed">
+  //           <parent link="${connected_to}"/>
+  //           <child link="${ns}_hand"/>
+  //           <origin xyz="${xyz}" rpy="${rpy}"/>
+  //         </joint>
+  //       </xacro:unless>
+  //       <!-- other things... -->
+  //       </xacro:macro>
+  //     </robot>
+  //
+  // Its use in franka_description/robots/panda_arm_hand.urdf.xacro
+  //     <xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
+  gripper_orientation.setRPY(0, 0, -M_PI_4);
+  // If we did not know it, we could also look it up!
+  // tf2_ros::Buffer tfBuffer;
+  // tf2_ros::TransformListener tfListener(tfBuffer);
+  // ros::WallDuration(0.1).sleep(); // wait a bit for the tf listener
+  // tf2::fromMsg((tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform).rotation, gripper_orientation);
+
+  // approach orientation
+  double approach_roll, approach_pitch, approach_yaw;
+  approach_roll =  -M_PI_2; // The neutral rotation has the gripper "looking down",
+  approach_pitch = -M_PI_4; // with these transformations, it grasps from one side.
+  approach_yaw =   -M_PI_4; // This results in (x=-0.5,y=-0.5,z=0,w=0.7071068)
+  tf2::Quaternion approach_orientation;
+  approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw);
+
+  tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation;
+
   grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
   grasps[0].grasp_pose.pose.position.x = 0.4;
   grasps[0].grasp_pose.pose.position.y = 0;
@@ -150,14 +192,14 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group)
   // Set support surface as table1.
   move_group.setSupportSurfaceName("table1");
   // Call pick to pick up the object using the grasps given
-  move_group.pick("object", grasps);
+  return move_group.pick("object", grasps);
 }
 
 /**
  * Place an object at a fixed position on top of table2.
  * @param group
  */
-void place(moveit::planning_interface::MoveGroupInterface& group)
+moveit::planning_interface::MoveItErrorCode place(moveit::planning_interface::MoveGroupInterface& group)
 {
   std::vector<moveit_msgs::PlaceLocation> place_location;
   place_location.resize(1);
@@ -200,7 +242,7 @@ void place(moveit::planning_interface::MoveGroupInterface& group)
   // Set support surface as table2.
   group.setSupportSurfaceName("table2");
   // Call place to place the object using the place locations given.
-  group.place("object", place_location);
+  return group.place("object", place_location);
 }
 
 /**
@@ -342,34 +384,5 @@ void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& pla
   planning_scene_interface.applyCollisionObjects(collision_objects);
 }
 
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "panda_arm_pick_place");
-  ros::NodeHandle nh;
-  ros::AsyncSpinner spinner(1);
-
-  ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
-  ros::Duration(3.0).sleep();
-  ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
-
-  spinner.start();
+} // end namespace pick_place_planner
 
-  ros::WallDuration(1.0).sleep();
-  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-  moveit::planning_interface::MoveGroupInterface group("panda_arm");
-  group.setPlanningTime(45.0);
-
-  addCollisionObjects(planning_scene_interface);
-
-  // Wait a bit for ROS things to initialize
-  ros::WallDuration(1.0).sleep();
-
-  pick(group);
-
-  ros::WallDuration(1.0).sleep();
-
-  place(group);
-
-  ros::waitForShutdown();
-  return 0;
-}

src/PickPlacePlanner.h

+
+#ifndef SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H
+#define SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H
+
+// ROS
+#include <ros/ros.h>
+
+// MoveIt
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+namespace pick_place_planner {
+
+/**
+ * Sets the posture argument to an open state for the two gripper fingers (8cm apart).
+ * @param posture An empty posture that is filled with a posture for two fingers
+ */
+void openGripper(trajectory_msgs::JointTrajectory &posture);
+
+/**
+ * Sets the posture argument to a closed state for the two gripper fingers
+ * @param posture An empty posture that is filled with a posture for two fingers
+ */
+void closedGripper(trajectory_msgs::JointTrajectory &posture);
+
+/**
+ * Pick an object from a fixed position.
+ * @param move_group The move group the pick operation is performed on
+ */
+moveit::planning_interface::MoveItErrorCode pick(moveit::planning_interface::MoveGroupInterface &move_group);
+
+/**
+* Place an object at a fixed position on top of table2.
+* @param group
+*/
+moveit::planning_interface::MoveItErrorCode place(moveit::planning_interface::MoveGroupInterface &group);
+
+/**
+* Add the collision objects to the planning scene and to gazebo.
+* @param planning_scene_interface The interface to access the planning scene.
+*/
+void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface &planning_scene_interface);
+
+}
+
+#endif // SAMPLE_APPLICATIONS_PICK_PLACE_PLANNER_H
\ No newline at end of file

src/PickPlacePlannerMain.cpp

+#include "PickPlacePlanner.h"
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "panda_arm_pick_place");
+    ros::NodeHandle nh;
+    ros::AsyncSpinner spinner(1);
+
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
+    ros::Duration(3.0).sleep();
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
+
+    spinner.start();
+
+    ros::WallDuration(1.0).sleep();
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+    moveit::planning_interface::MoveGroupInterface group("panda_arm");
+    group.setPlanningTime(45.0);
+
+    pick_place_planner::addCollisionObjects(planning_scene_interface);
+
+    // Wait a bit for ROS things to initialize
+    ros::WallDuration(1.0).sleep();
+
+    pick_place_planner::pick(group);
+
+    ros::WallDuration(1.0).sleep();
+
+    pick_place_planner::place(group);
+
+    ros::waitForShutdown();
+    return 0;
+}
\ No newline at end of file

test/grasping.test

+<launch>
+    <include file="$(find panda_simulation)/launch/simulation.launch">
+        <arg name="slow_simulation" value="false" />
+    </include>
+
+    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+          args="-d $(find sample_applications)/test/plan_visualizer.rviz" output="screen">
+        <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+    </node>
+
+    <test test-name="PickPlacePlannerTest" pkg="sample_applications" type="tests_grasping"  time-limit="3600.0" />
+</launch>
\ No newline at end of file

test/plan_visualizer.rviz

+Panels:
+  []
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Acceleration_Scaling_Factor: 1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Goal_Tolerance: 0
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: true
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: true
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: move_group/display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: panda_arm
+        Query Goal State: false
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: false
+      Value: true
+      Velocity_Scaling_Factor: 1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz_visual_tools/KeyTool
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 2.9803097248077393
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.0864897966384888
+        Y: 0.3387787342071533
+        Z: 2.2351798634190345e-7
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.270203560590744
+      Target Frame: panda_link0
+      Value: XYOrbit (rviz)
+      Yaw: 0.42677485942840576
+    Saved: ~
+Window Geometry:
+  Height: 1404
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000002000000000000013b000004e1fc0200000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000080000003640000017200fffffffb0000000800480065006c00700100000334000000e10000000000000000fb0000000a00560069006500770073000000003b000004e1000000000000000000000003000004fe0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000000000004fe0000010000ffffff000004fe000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Width: 1278
+  X: 0
+  Y: 18

test/test.cpp

+#include "PickPlacePlanner.h"
+
+#include <gtest/gtest.h>
+
+class Grasping : public ::testing::Test {
+public:
+    Grasping(): spinner(nullptr) {};
+
+protected:
+
+    ros::AsyncSpinner* spinner;
+    ros::NodeHandle* node;
+
+    void SetUp() override {
+        ::testing::Test::SetUp();
+
+        this->node = new ros::NodeHandle("~");
+        this->spinner = new ros::AsyncSpinner(2);
+        this->spinner->start();
+
+        ROS_INFO(">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
+        ros::Duration(3.0).sleep();
+        ROS_INFO(">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
+
+        // clean up scene
+        CleanupScene();
+    }
+
+
+    static void CleanupScene() {
+        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+
+        for (auto p : planning_scene_interface.getObjects()) {
+            p.second.operation = moveit_msgs::CollisionObject::REMOVE;
+            planning_scene_interface.applyCollisionObject(p.second);
+        }
+        ASSERT_EQ(planning_scene_interface.getObjects().size(), 0);
+        ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);
+    }
+
+    void TearDown() override
+    {
+        ros::shutdown();
+        delete this->spinner;
+        delete this->node;
+        ::testing::Test::TearDown();
+    }
+
+};
+
+
+TEST_F(Grasping, PickPlacePlannerTest) {
+    ros::WallDuration(1.0).sleep();
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+    moveit::planning_interface::MoveGroupInterface group("panda_arm");
+    group.setPlanningTime(45.0);
+
+    pick_place_planner::addCollisionObjects(planning_scene_interface);
+
+    // Wait a bit for ROS things to initialize
+    ros::WallDuration(1.0).sleep();
+
+    ASSERT_EQ(pick_place_planner::pick(group), moveit::planning_interface::MoveItErrorCode::SUCCESS);
+
+    ros::WallDuration(1.0).sleep();
+
+    ASSERT_EQ(pick_place_planner::place(group), moveit::planning_interface::MoveItErrorCode::SUCCESS);
+
+}
+
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "panda_arm_pick_place_test");
+    ::testing::InitGoogleTest(&argc, argv);
+
+    int result = RUN_ALL_TESTS();
+
+    return result;
+}
+
+