autoformat code

README.md

 # Panda Grasping Package
 
-Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This package uses the old pick-and-place pipeline of MoveIt.
+Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This
+package uses the old pick-and-place pipeline of MoveIt.
 
 ## How to install
 
@@ -13,6 +14,9 @@ Allows to pick and place objects. The current maximum height, where the panda ca
 
 * u can start the configurable pick and place with: ```roslaunch panda_grasping simulation.launch```
     * to pick or place open a second console
-    * to pick type for example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
-    * to place type for example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
-    * inside of the message u can update orientation (currenly only z is supported), position and size of the object to pick or place
+    * to pick type for
+      example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
+    * to place type for
+      example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'```
+    * inside of the message u can update orientation (currenly only z is supported), position and size of the object to
+      pick or place

include/environment_util.h

@@ -18,10 +18,9 @@
 /**
  * Util for the construction of the scene.
  */
-class EnvironmentUtil {
-
+class EnvironmentUtil
+{
 public:
-
   /**
    * Adds a table plate to the planning scene.
    * @param collision_objects current collision objects in the planning scene
@@ -51,9 +50,9 @@ public:
    * @param pose pose of the object to be picked
    * @param dimensions dimensions of the object to be picked
    */
-    moveit_msgs::CollisionObject
-    constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
-                          geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions);
+  moveit_msgs::CollisionObject constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects,
+                                                     std::string id, geometry_msgs::Pose& pose,
+                                                     geometry_msgs::Vector3& dimensions);
 };
 
 #endif  // PANDA_GRASPING_ENVIRONMENT_UTIL_H

include/franka_gripper_util.h

@@ -5,16 +5,13 @@
 #ifndef PUBLIC_FRANKA_GRIPPER_UTIL_H
 #define PUBLIC_FRANKA_GRIPPER_UTIL_H
 
-
-class FrankaGripperUtil {
-
+class FrankaGripperUtil
+{
 public:
   /**
    * resets the real robot, to allow correct future movements and grasps
    */
   void resetGripperForNextAction();
-
 };
 
-
 #endif  // PUBLIC_FRANKA_GRIPPER_UTIL_H

include/grasp_util.h

@@ -18,10 +18,9 @@
 /**
  * Util for grasping.
  */
-class GraspUtil {
-
+class GraspUtil
+{
 public:
-
   /**
    * distance of the center of the hand to the center of the finger tip
    */
@@ -56,11 +55,11 @@ public:
    * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported)
    * @param dimensions of the object
    * @param gripper_orientation current gripper orientation relative to the link it is attached to
-     * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner)
+   * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at
+   * the object's corner)
    * @return the "pre-pick"-pose
    */
-    geometry_msgs::Pose
-    getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions,
+  geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose& object_to_pick_pose, geometry_msgs::Vector3 dimensions,
                                            geometry_msgs::Quaternion gripper_orientation,
                                            double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
 
@@ -88,7 +87,8 @@ public:
    * @param object_to_place_id ID of the object to place
    */
   bool placeFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, geometry_msgs::Pose& place_pose,
-                        double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
+                      double open_amount, std::string supporting_surface_id, std::string object_to_place_id,
+                      bool plan_only = false);
 
   /**
    * Homes the gripper (in case it was abused before and is in an inconsistent state)
@@ -96,7 +96,6 @@ public:
    * @return true if homing succeeded, false otherwise
    */
   static bool homeGripper();
-
 };
 
 #endif  // PANDA_GRASPING_GRASP_UTIL_H

src/grasping/environment_util.cpp

@@ -6,8 +6,8 @@
 #include "environment_util.h"
 
 void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension,
-                                     double y_dimension) {
-
+                                     double y_dimension)
+{
   moveit_msgs::CollisionObject plate;
 
   plate.header.frame_id = "panda_link0";
@@ -32,8 +32,8 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
 
 void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
                                        double object_x_dimension, double object_y_dimension, double object_z_dimension,
-                                       geometry_msgs::Pose &object_to_pick_pose) {
-
+                                       geometry_msgs::Pose& object_to_pick_pose)
+{
   moveit_msgs::CollisionObject pick_support;
 
   pick_support.header.frame_id = "panda_link0";
@@ -57,9 +57,9 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
 }
 
 moveit_msgs::CollisionObject
-EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects,
-                                       std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions) {
-
+EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id,
+                                       geometry_msgs::Pose& pose, geometry_msgs::Vector3& dimensions)
+{
   moveit_msgs::CollisionObject object_to_pick;
 
   object_to_pick.header.frame_id = "panda_link0";

src/grasping/franka_gripper_util.cpp

@@ -10,8 +10,8 @@
 // Franka
 #include <franka_gripper/MoveAction.h>
 
-void FrankaGripperUtil::resetGripperForNextAction() {
-
+void FrankaGripperUtil::resetGripperForNextAction()
+{
   actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
 
   ROS_INFO("Waiting for reset action server to start.");
@@ -26,10 +26,13 @@ void FrankaGripperUtil::resetGripperForNextAction() {
   // wait for the action to return
   bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));
 
-    if (finished_before_timeout) {
+  if (finished_before_timeout)
+  {
     actionlib::SimpleClientGoalState state = ac.getState();
     ROS_INFO("Reset action finished: %s", state.toString().c_str());
-    } else {
+  }
+  else
+  {
     ROS_INFO("Reset action did not finish before the time out.");
   }
 }

src/grasping/grasp_service.cpp

@@ -16,7 +16,8 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/transform_listener.h>
 
-namespace grasping_state {
+namespace grasping_state
+{
 bool object_picked = false;
 }
 
@@ -33,13 +34,12 @@ tf2_ros::Buffer tfBuffer;
  * @param group a initialized MoveGroupInterface instance
  * @param n a valid nodehandle
  */
-bool pickObject(panda_grasping::PickObject::Request &req,
-                panda_grasping::PickObject::Response &res, ros::NodeHandle &n,
+bool pickObject(panda_grasping::PickObject::Request& req, panda_grasping::PickObject::Response& res, ros::NodeHandle& n,
                 moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
-                moveit::planning_interface::MoveGroupInterface &group) {
-
-    if (!grasping_state::object_picked) {
-
+                moveit::planning_interface::MoveGroupInterface& group)
+{
+  if (!grasping_state::object_picked)
+  {
     grasping_state::object_picked = true;
 
     EnvironmentUtil env_util;
@@ -51,30 +51,32 @@ bool pickObject(panda_grasping::PickObject::Request &req,
     env_util.constructPlate(collision_objects, 3.0, 3.0);
 
     ROS_INFO("Adding pick support.");
-        env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y,
-                                  req.dimensions.z,
+    env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
                               req.pose);
 
     ROS_INFO("Adding object.");
-        moveit_msgs::CollisionObject po = env_util.constructObjectToPick(collision_objects, "pick_object", req.pose,
-                                                                         req.dimensions);
+    moveit_msgs::CollisionObject po =
+        env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions);
 
     ROS_INFO("Applying to scene.");
-        if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
+    if (!planning_scene_interface.applyCollisionObjects(collision_objects))
+    {
       res.success = false;
       return false;
     }
 
     ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand.");
-        geometry_msgs::Transform gripper_transform = tfBuffer.lookupTransform("panda_hand", "panda_link8",
-                                                                                     ros::Time(0)).transform;
+    geometry_msgs::Transform gripper_transform =
+        tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform;
 
     ROS_INFO("Computing pre-grasp-pose.");
-        geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
+    geometry_msgs::Pose pick_pose =
+        grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
 
     ROS_INFO("Picking up ...");
     // 0.08 = 8cm = maximum gripper opening
-        if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) {
+    if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only))
+    {
       res.success = false;
       return false;
     }
@@ -96,32 +98,33 @@ bool pickObject(panda_grasping::PickObject::Request &req,
  * @param group a initialized MoveGroupInterface instance
  * @param n a valid nodehandle
  */
-bool
-placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObject::Response &res, ros::NodeHandle &n,
-            moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
-            moveit::planning_interface::MoveGroupInterface &group) {
-
+bool placeObject(panda_grasping::PlaceObject::Request& req, panda_grasping::PlaceObject::Response& res,
+                 ros::NodeHandle& n, moveit::planning_interface::PlanningSceneInterface& planning_scene_interface,
+                 moveit::planning_interface::MoveGroupInterface& group)
+{
   ROS_INFO_STREAM("received new place-request: " << req);
 
-    if (grasping_state::object_picked) {
+  if (grasping_state::object_picked)
+  {
     EnvironmentUtil env_util;
     GraspUtil grasp_util;
 
     std::vector<moveit_msgs::CollisionObject> collision_objects;
 
     ROS_INFO("Adding place support.");
-        env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y,
-                                  req.dimensions.z,
+    env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, req.dimensions.z,
                               req.pose);
 
     ROS_INFO("Applying to scene.");
-        if (!planning_scene_interface.applyCollisionObjects(collision_objects)) {
+    if (!planning_scene_interface.applyCollisionObjects(collision_objects))
+    {
       res.success = false;
       return false;
     }
 
     ROS_INFO("Placing object.");
-        if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) {
+    if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only))
+    {
       res.success = false;
       return false;
     }
@@ -136,9 +139,8 @@ placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObje
   return false;
 }
 
-
-int main(int argc, char **argv) {
-
+int main(int argc, char** argv)
+{
   ros::init(argc, argv, "grasp_service");
   ros::NodeHandle n("panda_grasping");
   ros::AsyncSpinner spinner(2);
@@ -149,12 +151,13 @@ int main(int argc, char **argv) {
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
   moveit::planning_interface::MoveGroupInterface group("panda_arm");
 
-    ros::ServiceServer pick_object_service = n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>(
+  ros::ServiceServer pick_object_service =
+      n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>(
           "PickObjectService",
           boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
 
-
-    ros::ServiceServer place_object_service = n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
+  ros::ServiceServer place_object_service =
+      n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>(
           "PlaceObjectService",
           boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group)));
 
@@ -162,4 +165,3 @@ int main(int argc, char **argv) {
 
   return 0;
 }
-

src/grasping/grasp_util.cpp

@@ -7,7 +7,8 @@
 #include <franka_gripper/MoveAction.h>
 #include <franka_gripper/HomingGoal.h>
 
-void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
+void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
   posture.joint_names.resize(2);
   posture.joint_names[0] = "panda_finger_joint1";
   posture.joint_names[1] = "panda_finger_joint2";
@@ -19,7 +20,8 @@ void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, doub
   posture.points[0].time_from_start = ros::Duration(0.5);
 }
 
-void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
+void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount)
+{
   posture.joint_names.resize(2);
   posture.joint_names[0] = "panda_finger_joint1";
   posture.joint_names[1] = "panda_finger_joint2";
@@ -31,10 +33,10 @@ void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, do
   posture.points[0].time_from_start = ros::Duration(0.5);
 }
 
-geometry_msgs::Pose
-GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions,
-                                geometry_msgs::Quaternion gripper_orientation_msg, double reach) {
-
+geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose& object_pose_msg,
+                                                    geometry_msgs::Vector3 dimensions,
+                                                    geometry_msgs::Quaternion gripper_orientation_msg, double reach)
+{
   // object orientation
   tf2::Quaternion object_orientation;
   tf2::fromMsg(object_pose_msg.orientation, object_orientation);
@@ -55,12 +57,16 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
 
   // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a
   // finger length is considered a safety distance.
-    if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE) {
-        ROS_WARN_STREAM("Reach around object reduced from " << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH -
-                                                                                  FRANKA_GRIPPER_FINGERTIP_SIZE << ".");
+  if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE)
+  {
+    ROS_WARN_STREAM("Reach around object reduced from "
+                    << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE << ".");
     reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE;
-    } else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2) {
-        ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0." << ". Minimum allowed reach is "
+  }
+  else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2)
+  {
+    ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0."
+                                                          << ". Minimum allowed reach is "
                                                           << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << ".");
     reach = 0;
   }
@@ -75,11 +81,10 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
   return target_pose;
 }
 
-bool
-GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
-                         geometry_msgs::Vector3 dimensions,
-                         double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
-
+bool GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group,
+                              geometry_msgs::Pose& pick_pose, geometry_msgs::Vector3 dimensions, double open_amount,
+                              std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only)
+{
   std::vector<moveit_msgs::Grasp> grasps;
   grasps.resize(1);
   grasps[0].grasp_pose.header.frame_id = "panda_link0";
@@ -101,15 +106,18 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
 
   setupOpenGripper(grasps[0].pre_grasp_posture, open_amount);
 
-    if (dimensions.x <= 0.08) {
+  if (dimensions.x <= 0.08)
+  {
     setupClosedGripper(grasps[0].grasp_posture, dimensions.x);
   }
 
-    if (dimensions.x > 0.08 && dimensions.y <= 0.08) {
+  if (dimensions.x > 0.08 && dimensions.y <= 0.08)
+  {
     setupClosedGripper(grasps[0].grasp_posture, dimensions.y);
   }
 
-    if (dimensions.x > 0.08 && dimensions.y > 0.08) {
+  if (dimensions.x > 0.08 && dimensions.y > 0.08)
+  {
     ROS_ERROR("Could not grasp: Object to big (in any dimension).");
     return false;
   }
@@ -118,7 +126,8 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
 
   moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
 
-    if (miec.val == miec.SUCCESS) {
+  if (miec.val == miec.SUCCESS)
+  {
     return true;
   }
 
@@ -126,10 +135,10 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
   return false;
 }
 
-bool
-GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
-                          double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only) {
-
+bool GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface& move_group,
+                               geometry_msgs::Pose& place_pose, double open_amount, std::string supporting_surface_id,
+                               std::string object_to_place_id, bool plan_only)
+{
   std::vector<moveit_msgs::PlaceLocation> place_location;
   place_location.resize(1);
 
@@ -154,7 +163,8 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
 
   moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
 
-    if (miec.val == miec.SUCCESS) {
+  if (miec.val == miec.SUCCESS)
+  {
     return true;
   }
 
@@ -162,14 +172,17 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
   return false;
 }
 
-bool GraspUtil::homeGripper() {
+bool GraspUtil::homeGripper()
+{
   actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
-    if (moveActionClient.waitForServer(ros::Duration(1.0))) {
+  if (moveActionClient.waitForServer(ros::Duration(1.0)))
+  {
     moveActionClient.cancelAllGoals();
   }
 
   actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true);
-    if (!ac.waitForServer(ros::Duration(10.0))) {
+  if (!ac.waitForServer(ros::Duration(10.0)))
+  {
     ROS_ERROR("Gripper homing failed (action server not available).");
     return false;
   }
@@ -177,11 +190,14 @@ bool GraspUtil::homeGripper() {
   ac.cancelAllGoals();
   franka_gripper::HomingGoal goal;
   ac.sendGoal(goal);
-    if (ac.waitForResult(ros::Duration(15.0))) {
+  if (ac.waitForResult(ros::Duration(15.0)))
+  {
     actionlib::SimpleClientGoalState state = ac.getState();
     ROS_INFO_STREAM("Homing finished: " << state.toString());
     return state == actionlib::SimpleClientGoalState::SUCCEEDED;
-    } else {
+  }
+  else
+  {
     ROS_ERROR("Gripper homing failed (with timeout).");
     return false;
   }