diff --git a/CMakeLists.txt b/CMakeLists.txt
index f8eeccd70ba49ec518786a5e8735ee486fcddaee..ae98355e3f18b0c3221cf81e8086c025f5ce9a69 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/package.xml b/package.xml
index 90cc3dea1b768399373584fd7020b852982c858f..bb19ef9d403c4caf40b0e37a3d727af699280993 100644
--- a/package.xml
+++ b/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>
diff --git a/src/PickPlacePlanner.cpp b/src/PickPlacePlanner.cpp
index 0dcd019720a5976b08828c8220786432a625b1e7..613ad999aba753e723690a2f821470d4ab915bde 100644
--- a/src/PickPlacePlanner.cpp
+++ b/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;
-}
diff --git a/src/PickPlacePlanner.h b/src/PickPlacePlanner.h
new file mode 100644
index 0000000000000000000000000000000000000000..980c2a87ac3ce18e5025b80bfbdbe2a0ed2afa28
--- /dev/null
+++ b/src/PickPlacePlanner.h
@@ -0,0 +1,46 @@
+
+#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
diff --git a/src/PickPlacePlannerMain.cpp b/src/PickPlacePlannerMain.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b2248c05c5f487069daa2cc410049b14afcd9e1
--- /dev/null
+++ b/src/PickPlacePlannerMain.cpp
@@ -0,0 +1,33 @@
+#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
diff --git a/test/grasping.test b/test/grasping.test
new file mode 100644
index 0000000000000000000000000000000000000000..235eec5237727f5d265bf517341eaad5126462df
--- /dev/null
+++ b/test/grasping.test
@@ -0,0 +1,12 @@
+<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
diff --git a/test/plan_visualizer.rviz b/test/plan_visualizer.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..2f631356378c3b3dc72a04abff35b9b9d5f1893f
--- /dev/null
+++ b/test/plan_visualizer.rviz
@@ -0,0 +1,286 @@
+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
diff --git a/test/test.cpp b/test/test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..252882e836cc2ca082784b529350aedc3b9e7e88
--- /dev/null
+++ b/test/test.cpp
@@ -0,0 +1,82 @@
+#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;
+}
+
+