diff --git a/turtlebot_arm_object_manipulation/CMakeLists.txt b/turtlebot_arm_object_manipulation/CMakeLists.txt index b98fcb1..b216b31 100644 --- a/turtlebot_arm_object_manipulation/CMakeLists.txt +++ b/turtlebot_arm_object_manipulation/CMakeLists.txt @@ -4,7 +4,7 @@ project(turtlebot_arm_object_manipulation) SET(CMAKE_CXX_FLAGS "-std=c++0x") # setup -find_package(catkin REQUIRED actionlib actionlib_msgs interactive_markers object_recognition_msgs roscpp visualization_msgs arbotix_msgs moveit_msgs moveit_core moveit_ros_planning_interface) +find_package(catkin REQUIRED actionlib actionlib_msgs interactive_markers object_recognition_msgs roscpp visualization_msgs arbotix_msgs moveit_msgs moveit_core moveit_ros_planning_interface yocs_math_toolkit) find_package(Boost REQUIRED system filesystem) find_package(PCL REQUIRED) link_directories(${catkin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${PCL_LIBRARY_DIRS}) @@ -23,7 +23,7 @@ add_action_files(FILES ) generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs) -catkin_package(DEPENDS actionlib actionlib_msgs interactive_markers object_recognition_msgs roscpp visualization_msgs arbotix_msgs moveit_msgs moveit_core moveit_visual_tools moveit_ros_planning_interface) +catkin_package(DEPENDS actionlib actionlib_msgs interactive_markers object_recognition_msgs roscpp visualization_msgs arbotix_msgs moveit_msgs moveit_core moveit_visual_tools moveit_ros_planning_interface yocs_math_toolkit) # nodes diff --git a/turtlebot_arm_object_manipulation/package.xml b/turtlebot_arm_object_manipulation/package.xml index 8743df9..2fa6335 100644 --- a/turtlebot_arm_object_manipulation/package.xml +++ b/turtlebot_arm_object_manipulation/package.xml @@ -26,6 +26,7 @@ moveit_msgs moveit_core moveit_ros_planning_interface + yocs_math_toolkit roscpp actionlib @@ -37,4 +38,5 @@ moveit_msgs moveit_core moveit_ros_planning_interface + yocs_math_toolkit diff --git a/turtlebot_arm_object_manipulation/src/object_detection_action_server.cpp b/turtlebot_arm_object_manipulation/src/object_detection_action_server.cpp index f081432..78bf5db 100644 --- a/turtlebot_arm_object_manipulation/src/object_detection_action_server.cpp +++ b/turtlebot_arm_object_manipulation/src/object_detection_action_server.cpp @@ -35,6 +35,9 @@ #include #include +#include +#include + // action client: ORK's tabletop object recognition #include #include @@ -57,6 +60,8 @@ namespace turtlebot_arm_object_manipulation class ObjectDetectionServer { + class DetectionBin; // forward declaration of private class DetectionBin + private: // ROS interface @@ -65,6 +70,8 @@ class ObjectDetectionServer // Action client for the ORK object recognition and server actionlib::SimpleActionClient ork_ac_; + ros::Duration ork_execute_timeout_; + ros::Duration ork_preempt_timeout_; // Action server to handle it conveniently for our object manipulation demo actionlib::SimpleActionServer od_as_; @@ -83,6 +90,8 @@ class ObjectDetectionServer ros::Publisher c_obj_pub_; ros::Publisher scene_pub_; + std::vector table_poses_; + // We use the planning_scene_interface::PlanningSceneInterface to manipulate the world moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; @@ -92,19 +101,25 @@ class ObjectDetectionServer std::string arm_link_; double obj_size_; + // Object detection and classification constants + const double CONFIDENCE_THRESHOLD = 0.9; // minimum confidence required to accept an object + const double CLUSTERING_THRESHOLD = 0.05; // maximum acceptable distance to assign an object to a bin + const unsigned CALLS_TO_ORK_TABLETOP = 10; + public: ObjectDetectionServer(const std::string name) : - pnh_("~"), ork_ac_("tabletop/recognize_objects", true), od_as_(name, false), action_name_(name) + pnh_("~"), ork_ac_("tabletop/recognize_objects", true), od_as_(name, false), action_name_(name), + ork_execute_timeout_(5.0), ork_preempt_timeout_(1.0) { - // create the action client; spin its own thread + // Create the action client; spin its own thread - // wait for the tabletop/recognize_objects action server to start before we provide our own service + // Wait for the tabletop/recognize_objects action server to start before we provide our own service ROS_INFO("[object detection] Waiting for tabletop/recognize_objects action server to start..."); ork_ac_.waitForServer(); ROS_INFO("[object detection] tabletop/recognize_objects action server started; ready for sending goals."); - // wait for the get object information service + // Wait for the get object information service // CANNOT FIND IT!!! XXX obj_info_srv_ = nh_.serviceClient("add_two_ints"); // TODO: I must do it by myself... based on object_search.py script @@ -128,54 +143,99 @@ class ObjectDetectionServer { ROS_INFO("[object detection] Received goal!"); - // accept the new goal + // Accept the new goal goal_ = od_as_.acceptNewGoal(); arm_link_ = goal_->frame; obj_size_ = goal_->obj_size; + // Clear results from previous goals + table_poses_.clear(); result_.obj_poses.poses.clear(); result_.obj_names.clear(); result_.obj_poses.header.frame_id = arm_link_; - // send a goal to the action server - ROS_INFO("[object detection] Sending goal to tabletop/recognize_objects action server."); - object_recognition_msgs::ObjectRecognitionGoal goal; - ork_ac_.sendGoal(goal); - - // wait for the action to return - bool finished_before_timeout = ork_ac_.waitForResult(ros::Duration(10.0)); + planning_scene_interface_.removeCollisionObjects(planning_scene_interface_.getKnownObjectNames()); - if (finished_before_timeout) - { - actionlib::SimpleClientGoalState state = ork_ac_.getState(); - ROS_INFO("[object detection] Action finished: %s",state.toString().c_str()); - } - else - { - ROS_WARN("[object detection] Tabletop/recognize_objects action did not finish before the time out."); - od_as_.setAborted(result_, "Tabletop/recognize_objects action did not finish before the time out"); - return; - } - object_recognition_msgs::ObjectRecognitionResultConstPtr result = ork_ac_.getResult(); + // Call ORK tabletop action server and wait for the action to return + // We do it CALLS_TO_TABLETOP times and accumulate the results on bins to provide more reliable results + std::vector detection_bins; - // Add all detected objects to the goal result; TODO do some filtering!!! - for (object_recognition_msgs::RecognizedObject obj: result->recognized_objects.objects) + ROS_INFO("[object detection] Sending %d goals to tabletop/recognize_objects action server...", + CALLS_TO_ORK_TABLETOP); + object_recognition_msgs::ObjectRecognitionGoal goal; + for (int i = 0; i < CALLS_TO_ORK_TABLETOP; ++i) { - addBlock(obj); - } - - if (result_.obj_poses.poses.size() > 0) + actionlib::SimpleClientGoalState state = + ork_ac_.sendGoalAndWait(goal, ork_execute_timeout_, ork_preempt_timeout_); + + if (state == actionlib::SimpleClientGoalState::SUCCEEDED) + { + ROS_INFO("[object detection] Action successfully finished"); + } + else + { + ROS_WARN("[object detection] Tabletop/recognize_objects action did not finish before the time out: %s", + state.toString().c_str()); + od_as_.setAborted(result_, "Tabletop/recognize_objects action did not finish before the time out"); + continue; + } + + object_recognition_msgs::ObjectRecognitionResultConstPtr result = ork_ac_.getResult(); + + // Classify objects detected in each call to tabletop into bins based on distance to bin's centroid + for (const object_recognition_msgs::RecognizedObject& obj: result->recognized_objects.objects) + { + if (obj.confidence < CONFIDENCE_THRESHOLD) + continue; + + bool assigned = false; + for (DetectionBin& bin: detection_bins) + { + if (mtk::distance3D(bin.getCentroid().pose, obj.pose.pose.pose) <= CLUSTERING_THRESHOLD) + { + ROS_DEBUG("Object with pose [%s] added to bin %d with centroid [%s] with distance [%f]", + mtk::pose2str3D(obj.pose.pose.pose).c_str(), bin.id, mtk::pose2str3D(bin.getCentroid()).c_str(), + mtk::distance3D(bin.getCentroid().pose, obj.pose.pose.pose)); + bin.addObject(obj); + assigned = true; + break; + } + } + + if (! assigned) + { + // No matching bin; create a new one for this object + ROS_DEBUG("Object with pose [%s] added to a new bin", mtk::pose2str3D(obj.pose.pose.pose).c_str()); + DetectionBin new_bin; + new_bin.id = detection_bins.size(); + new_bin.addObject(obj); + detection_bins.push_back(new_bin); + } + } + + ros::spinOnce(); // keep spinning so table messages callbacks are processed + } // loop up to CALLS_TO_ORK_TABLETOP + + // Add a detected object per bin to the goal result, if the bin is consistent enough + int added_objects = addObjects(detection_bins); + if (added_objects > 0) { block_pub_.publish(result_.obj_poses); - ROS_INFO("[object detection] Set as succeeded!"); + ROS_INFO("[object detection] Succeeded! %d objects detected", added_objects); } else { - ROS_INFO("[object detection] Couldn't find any blocks this iteration!"); + ROS_INFO("[object detection] Succeeded, but ouldn't find any blocks this iteration"); } + // Add also the table as a collision object, so it gets filtered out from MoveIt! octomap + if (table_poses_.size() > 0) + addTable(table_poses_); + else + ROS_WARN("[object detection] No near-horizontal table detected!"); + od_as_.setSucceeded(result_); } @@ -194,69 +254,107 @@ class ObjectDetectionServer return; } - // Add the table as a collision object, so it gets filtered out from MoveIt! octomap - for (object_recognition_msgs::Table table: msg.tables) + // Accumulate table poses while detecting objects so the resulting pose (centroid) is more accurate + for (const object_recognition_msgs::Table& table: msg.tables) { - addTable(table); + geometry_msgs::Pose table_pose = table.pose; + // Tables often have orientations with all-nan values, but assertQuaternionValid lets them go! + if (isnan(table.pose.orientation.x) || + isnan(table.pose.orientation.y) || + isnan(table.pose.orientation.z) || + isnan(table.pose.orientation.w)) + { + ROS_WARN("[object detection] Table discarded as its orientation has nan values"); + continue; + } + + if (! transformPose(msg.header.frame_id, arm_link_, table.pose, table_pose)) + { + continue; + } + + if ((std::abs(mtk::roll(table_pose)) < M_PI/10.0) && (std::abs(mtk::pitch(table_pose)) < M_PI/10.0)) + { + // Only consider tables within +/-18 degrees away from the horizontal plane + table_poses_.push_back(table_pose); + } + else + { + ROS_WARN("Table with pose [%s] discarded as it is %.2f radians away from the horizontal plane", + mtk::pose2str3D(table_pose).c_str(), + std::max(std::abs(mtk::roll(table_pose)), std::abs(mtk::pitch(table_pose)))); + } } } private: - void addBlock(const object_recognition_msgs::RecognizedObject& obj) + int addObjects(const std::vector& detection_bins) { - if (obj.confidence < 0.9) - return; - - std::string obj_name; - std::stringstream sstream; - sstream << "block_" << result_.obj_names.size() + 1; - obj_name = sstream.str(); - - geometry_msgs::Pose out_pose; - transformPose(obj.header.frame_id, arm_link_, obj.pose.pose.pose, out_pose); - ROS_INFO_STREAM("[object detection] Adding \"" << obj_name << "\" object at " - << out_pose.position.x << ", " - << out_pose.position.y << ", " - << out_pose.position.z); - result_.obj_poses.poses.push_back(out_pose); - result_.obj_names.push_back(obj_name); - - moveit_msgs::CollisionObject co; - co.header = result_.obj_poses.header; - co.id = obj_name; - planning_scene_interface_.removeCollisionObjects(std::vector(1, co.id)); - - co.operation = moveit_msgs::CollisionObject::ADD; - co.primitives.resize(1); - co.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - co.primitives[0].dimensions.resize(shape_tools::SolidPrimitiveDimCount::value); - co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = obj_size_; - co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = obj_size_; - co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = obj_size_; - co.primitive_poses.resize(1); - co.primitive_poses[0] = out_pose; + std::vector collision_objects; + moveit_msgs::PlanningScene ps; + ps.is_diff = true; -// if (! transformPose(table.header.frame_id, arm_link_, table.pose, co.primitive_poses[0])) -// return; -// -// co.primitive_poses[0].position.x += table_size_x/3.0; -// co.primitive_poses[0].position.z -= table_size_z/2.0; + // Add a detected object per bin to the goal result and to the planning scene as a collision object + // Only bins receiving detections on most of the ORK tabletop calls are considered consistent enough + for (const DetectionBin& bin: detection_bins) + { + if (bin.countObjects() < CALLS_TO_ORK_TABLETOP/1.5) + { + ROS_DEBUG("Bin %d with centroid [%s] discarded as it received %d objects out of %d attempts", + bin.id, mtk::pose2str3D(bin.getCentroid()).c_str(), bin.countObjects(), CALLS_TO_ORK_TABLETOP); + continue; + } + + // TODO: invented name; get somehow from db using the key + std::string obj_name; + std::stringstream sstream; + sstream << "block_" << result_.obj_names.size() + 1; + obj_name = sstream.str(); + + ROS_DEBUG("Bin %d with centroid [%s] and %d objects added as object %s", + bin.id, mtk::pose2str3D(bin.getCentroid()).c_str(), bin.countObjects(), obj_name.c_str()); + + geometry_msgs::Pose out_pose; + transformPose(bin.getCentroid().header.frame_id, arm_link_, bin.getCentroid().pose, out_pose); + ROS_INFO("[object detection] Adding \"%s\" object at %s", obj_name.c_str(), mtk::point2str(out_pose.position)); + result_.obj_poses.poses.push_back(out_pose); + result_.obj_names.push_back(obj_name); + + moveit_msgs::CollisionObject co; + co.header = result_.obj_poses.header; + co.id = obj_name; + + co.operation = moveit_msgs::CollisionObject::ADD; + co.primitives.resize(1); + co.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + co.primitives[0].dimensions.resize(shape_tools::SolidPrimitiveDimCount::value); + co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = obj_size_; + co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = obj_size_; + co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = obj_size_; + co.primitive_poses.resize(1); + co.primitive_poses[0] = out_pose; + + collision_objects.push_back(co); + + // Provide a random color to the collision object + moveit_msgs::ObjectColor oc; + oc.id = co.id; + oc.color = getRandColor(1.0); + ps.object_colors.push_back(oc); + } - std::vector collision_objects(1, co); - planning_scene_interface_.addCollisionObjects(collision_objects); + if (collision_objects.size() > 0) + { + planning_scene_interface_.addCollisionObjects(collision_objects); + scene_pub_.publish(ps); + } - moveit_msgs::ObjectColor oc; - oc.id = co.id; - oc.color = getRandColor(1.0); - moveit_msgs::PlanningScene ps; - ps.object_colors.push_back(oc); - ps.is_diff = true; - scene_pub_.publish(ps); + return collision_objects.size(); } - void addTable(const object_recognition_msgs::Table& table) + void addTable(const std::vector& table_poses) { // Add the table as a collision object into the world, so it gets excluded from the collision map // As the blocks are small, they should also be excluded (assuming that padding_offset parameter on @@ -269,9 +367,7 @@ class ObjectDetectionServer moveit_msgs::CollisionObject co; co.header.stamp = ros::Time::now(); co.header.frame_id = arm_link_; - co.id = "table"; - planning_scene_interface_.removeCollisionObjects(std::vector(1, co.id)); co.operation = moveit_msgs::CollisionObject::ADD; co.primitives.resize(1); @@ -282,26 +378,33 @@ class ObjectDetectionServer co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = table_size_z; co.primitive_poses.resize(1); - if (! transformPose(table.header.frame_id, arm_link_, table.pose, co.primitive_poses[0])) - return; + // Calculate table_pose as the centroid of all accumulated poses + double roll_acc = 0.0, pitch_acc = 0.0, yaw_acc = 0.0; + for (const geometry_msgs::Pose& pose: table_poses) + { + ROS_DEBUG("[object detection] Adding a table at %s based on %d observations", + mtk::pose2str3D(pose).c_str(), table_poses.size()); + + co.primitive_poses[0].position.x += pose.position.x; + co.primitive_poses[0].position.y += pose.position.y; + co.primitive_poses[0].position.z += pose.position.z; + roll_acc += mtk::roll(pose); + pitch_acc += mtk::pitch (pose); + yaw_acc += mtk::yaw(pose); + } + co.primitive_poses[0].position.x /= (double)table_poses.size(); + co.primitive_poses[0].position.y /= (double)table_poses.size(); + co.primitive_poses[0].position.z /= (double)table_poses.size(); + co.primitive_poses[0].orientation = tf::createQuaternionMsgFromRollPitchYaw(roll_acc/(double)table_poses.size(), + pitch_acc/(double)table_poses.size(), + yaw_acc/(double)table_poses.size()); + // Displace the table center according to its harcoded size; TODO: remove once we estimate the table's size co.primitive_poses[0].position.x += table_size_x/3.0; co.primitive_poses[0].position.z -= table_size_z/2.0; - // Tables often have orientations with all-nan values, but assertQuaternionValid lets them go! - if (std::isnan(co.primitive_poses[0].orientation.x) || - std::isnan(co.primitive_poses[0].orientation.y) || - std::isnan(co.primitive_poses[0].orientation.z) || - std::isnan(co.primitive_poses[0].orientation.w)) - { - ROS_WARN("[object detection] Table orientation has nan values; assuming horizontal"); - co.primitive_poses[0].orientation = tf::createQuaternionMsgFromYaw(0.0); - } - - ROS_INFO_STREAM("[object detection] Adding a table at " << co.primitive_poses[0].position.x << ", " - << co.primitive_poses[0].position.y << ", " - << co.primitive_poses[0].position.z - << " as a collision object into the world"); + ROS_INFO("[object detection] Adding a table at %s as a collision object into the world", + mtk::point2str(co.primitive_poses[0].position)); std::vector collision_objects(1, co); planning_scene_interface_.addCollisionObjects(collision_objects); } @@ -353,10 +456,75 @@ class ObjectDetectionServer return color; } -}; + /** + * Private class for clustering together the same object detected in consecutive observations + * and provide a more accurate pose as the centroid of all observations. + */ + class DetectionBin + { + public: + unsigned int id; + unsigned int countObjects() const { return objects.size(); } + const geometry_msgs::PoseStamped& getCentroid() const { return centroid; } + + void addObject(object_recognition_msgs::RecognizedObject object) + { + objects.push_back(object); + + // recalculate centroid + centroid = geometry_msgs::PoseStamped(); + double confidence_acc = 0.0, roll_acc = 0.0, pitch_acc = 0.0, yaw_acc = 0.0; + for (const object_recognition_msgs::RecognizedObject& obj: objects) + { + centroid.header.stamp = ros::Time::now(); + centroid.header.frame_id = obj.pose.header.frame_id; // TODO could do some checking + + centroid.pose.position.x += obj.pose.pose.pose.position.x * obj.confidence; + centroid.pose.position.y += obj.pose.pose.pose.position.y * obj.confidence; + centroid.pose.position.z += obj.pose.pose.pose.position.z * obj.confidence; + roll_acc += mtk::roll(obj.pose.pose.pose) * obj.confidence; + pitch_acc += mtk::pitch (obj.pose.pose.pose) * obj.confidence; + yaw_acc += mtk::yaw(obj.pose.pose.pose) * obj.confidence; + + confidence_acc += obj.confidence; + } + + centroid.pose.position.x /= confidence_acc; + centroid.pose.position.y /= confidence_acc; + centroid.pose.position.z /= confidence_acc; + centroid.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll_acc/confidence_acc, + pitch_acc/confidence_acc, + yaw_acc/confidence_acc); + confidence = confidence_acc/(double)objects.size(); + } + + std::string getName() + { + std::map key_occurences; + for (const object_recognition_msgs::RecognizedObject& obj: objects) + //if (key_occurences. + key_occurences[obj.type.key]++; + + std::string commonest_key; + std::map::iterator it; + for(it = key_occurences.begin(); it != key_occurences.end(); it++) + { + if (it->second > key_occurences[commonest_key]) + commonest_key = it->first; + } + return commonest_key; + } + + private: + double confidence; + geometry_msgs::PoseStamped centroid; + std::vector objects; + }; }; +}; // namespace turtlebot_arm_object_manipulation + int main(int argc, char** argv) { ros::init(argc, argv, "object_detection_action_server"); diff --git a/turtlebot_arm_object_manipulation/src/pick_and_place_action_server.cpp b/turtlebot_arm_object_manipulation/src/pick_and_place_action_server.cpp index b160fa8..72f76e8 100644 --- a/turtlebot_arm_object_manipulation/src/pick_and_place_action_server.cpp +++ b/turtlebot_arm_object_manipulation/src/pick_and_place_action_server.cpp @@ -31,11 +31,14 @@ #include #include +#include + #include #include #include #include +#include #include #include @@ -57,7 +60,7 @@ class PickAndPlaceServer turtlebot_arm_object_manipulation::PickAndPlaceGoalConstPtr goal_; ros::Publisher target_pose_pub_; - ros::Subscriber pick_and_place_sub_; + ros::Subscriber planning_scene_sub_; // Move groups to control arm and gripper with MoveIt! moveit::planning_interface::MoveGroup arm_; @@ -65,6 +68,7 @@ class PickAndPlaceServer // We use the planning_scene_interface::PlanningSceneInterface to manipulate the world moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; + std::vector attached_collision_objs_; // Pick and place parameters std::string arm_link; @@ -94,6 +98,8 @@ class PickAndPlaceServer as_.start(); + planning_scene_sub_ = nh_.subscribe("/move_group/monitored_planning_scene", 1, &PickAndPlaceServer::sceneCB, this); + target_pose_pub_ = nh_.advertise("/target_pose", 1, true); } @@ -128,84 +134,26 @@ class PickAndPlaceServer as_.setPreempted(); } - - std::string pose2str3D(const geometry_msgs::Pose& pose) + void sceneCB(const moveit_msgs::PlanningScene& scene) { - tf::Transform tf; - tf::poseMsgToTF(pose, tf); - double roll, pitch, yaw; - tf::Matrix3x3(tf.getRotation()).getRPY(roll, pitch, yaw); - - char ___buffer___[100]; - sprintf(___buffer___, "%.2f, %.2f, %.2f, %.2f, %.2f, %.2f", - pose.position.x, pose.position.y, pose.position.z, - roll, pitch, yaw); - return std::string(___buffer___); + // Keep attached collision objects so we can subtract its pose from the place location poses + attached_collision_objs_ = scene.robot_state.attached_collision_objects; } -// 0.17, -0.01, 0.04, 0.67, 0.84, 0.74 -// 0.17, -0.01, 0.04, 1.30, -1.16, 1.75 -// -// -// 0.18, -0.12, 0.05, 0.13, 1.00, -2.61 -// 0.18, -0.12, 0.05, -0.98, -0.29, 1.76 -// 0.18, -0.12, 0.05, 0.59, -0.88, -0.33 -// 0.18, -0.12, 0.05, 0.93, -0.47, -0.89 -// 0.18, -0.12, 0.05, -1.01, -0.12, 1.85 -// 0.18, -0.12, 0.05, -0.52, 0.92, 2.89 - - - bool pickAndPlace(const std::string& obj_name, const geometry_msgs::Pose& pick_pose, const geometry_msgs::Pose& place_pose) { -// planning_scene_interface_. remove(std::vector(1, co.id)); -// scene.remove_attached_object(GRIPPER_FRAME, "block_1") - ROS_ERROR_STREAM(obj_name << " object " << arm_.getEndEffectorLink() << " "< ao_poses = planning_scene_interface_.getObjectPoses(std::vector(1, obj_name)); - for (std::pair ao_pose: ao_poses) + if (pick(obj_name, pick_pose) && place(obj_name, place_pose)) { - ROS_ERROR_STREAM("BEFORE PICK " << ao_pose.first << " object at " << pose2str3D(ao_pose.second) << " "<< arm_.getEndEffectorLink() << " "< ao_poses = planning_scene_interface_.getObjectPoses(std::vector(1, obj_name)); - for (std::pair ao_pose: ao_poses) - { - ROS_ERROR_STREAM("AFTER PICK " << ao_pose.first << " object at " << pose2str3D(ao_pose.second) << " "<< arm_.getEndEffectorLink() << " "<(1, obj_name)); - for (std::pair ao_pose: ao_poses) - { - ROS_ERROR_STREAM("AFTER ATTACH " << ao_pose.first << " object at " << pose2str3D(ao_pose.second) << " "<< arm_.getEndEffectorLink() << " "< ao_poses = planning_scene_interface_.getObjectPoses(std::vector(1, obj_name)); - for (std::pair ao_pose: ao_poses) - { - ROS_ERROR_STREAM("AFTER PLACE " << ao_pose.first << " object at " << pose2str3D(ao_pose.second) << " "<< arm_.getEndEffectorLink() << " "< ao_poses = planning_scene_interface_.getObjectPoses(std::vector(1, obj_name)); -// for (std::pair ao_pose: ao_poses) -// { -// ROS_ERROR_STREAM(ao_pose.first << " object at " << pose2str3D(ao_pose.second)); -// } - as_.setAborted(result_); - return false; -// } + as_.setAborted(result_); + return false; } bool pick(const std::string& obj_name, const geometry_msgs::Pose& pose) @@ -242,14 +190,12 @@ class PickAndPlaceServer g.grasp_pose = p; g.pre_grasp_approach.direction.vector.x = 0.5; - //g.pre_grasp_approach.direction.vector.z = -0.5; g.pre_grasp_approach.direction.header.frame_id = arm_.getEndEffectorLink(); g.pre_grasp_approach.min_distance = 0.005; g.pre_grasp_approach.desired_distance = 0.1; g.post_grasp_retreat.direction.header.frame_id = arm_.getEndEffectorLink(); g.post_grasp_retreat.direction.vector.x = -0.5; -// g.post_grasp_retreat.direction.vector.z = 0.5; g.post_grasp_retreat.min_distance = 0.005; g.post_grasp_retreat.desired_distance = 0.1; @@ -297,18 +243,34 @@ class PickAndPlaceServer return false; } + if (attached_collision_objs_.size() > 0) + { + // MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains + // the orientation it had when picked. However, with our 4-dofs arm this is infeasible (and also we don't care + // about the objects orientation), so we cancel this transformation. It is applied here: + // https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64 + // More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577 + geometry_msgs::Pose aco_pose = attached_collision_objs_[0].object.primitive_poses[0]; + + tf::Transform place_tf, aco_tf; + tf::poseMsgToTF(p.pose, place_tf); + tf::poseMsgToTF(aco_pose, aco_tf); + tf::poseTFToMsg(place_tf * aco_tf, p.pose); + + ROS_DEBUG("Compensate place pose with the attached object pose [%s]. Results: [%s]", + mtk::pose2str3D(aco_pose).c_str(), mtk::pose2str3D(p.pose).c_str()); + } + std::vector loc; moveit_msgs::PlaceLocation l; l.place_pose = p; l.pre_place_approach.direction.vector.x = 0.5; -// l.pre_place_approach.direction.vector.z = -0.1; l.pre_place_approach.direction.header.frame_id = arm_.getEndEffectorLink(); l.pre_place_approach.min_distance = 0.005; l.pre_place_approach.desired_distance = 0.1; l.post_place_retreat.direction.vector.x = -0.5; -// l.post_place_retreat.direction.vector.z = 0.1; l.post_place_retreat.direction.header.frame_id = arm_.getEndEffectorLink(); l.post_place_retreat.min_distance = 0.005; l.post_place_retreat.desired_distance = 0.1; @@ -357,9 +319,11 @@ class PickAndPlaceServer private: /** - * Move arm to a target pose. Only position coordinates are taken into account; the - * orientation is calculated according to the direction and distance to the target. - * @param target Pose target to achieve + * Convert a simple 3D point into a valid pick/place pose. The orientation Euler angles + * are calculated as a function of the x and y coordinates, plus some random variations + * Increasing with the number of attempts to improve our chances of successful planning. + * @param target Pose target to validate + * @param attempt The actual attempts number * @return True of success, false otherwise */ bool validateTargetPose(geometry_msgs::PoseStamped& target, int attempt = 0) @@ -410,11 +374,11 @@ class PickAndPlaceServer // the target. We also try some random variations of both to increase the chances of successful planning. // Roll is inverted with negative yaws so the arms doesn't make full turns when acting in different halfs // of the working space. - // XXX: We don't need to try different values of yaw as long as this issue is implemented (or hacked) : - // https://github.com/ros-planning/moveit_ros/issues/577 double rp = M_PI_2 - std::asin((d - 0.1)/0.205) + ((attempt%2)*2 - 1)*(std::ceil(attempt/2.0)*0.05); double ry = std::atan2(y, x); - double rr = ry < 0.0 ? M_PI : 0.0; + double rr = 0.0;///ry < 0.0 ? M_PI : 0.0; NO!!! IT IS IGNORED!!! +// if ((ry < 0.0 && ry >= -M_PI/2.0) || (ry >= M_PI/2.0)) +// rr = M_PI; target.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(rr, rp, ry); // Slightly increase z proportionally to pitch to avoid hitting the table with the lower gripper corner and @@ -425,8 +389,7 @@ class PickAndPlaceServer target.pose.position.z += z_delta1; target.pose.position.z += z_delta2; - ROS_DEBUG("[pick and place] Set pose target [%.2f, %.2f, %.2f] [d: %.2f, r: %.2f, p: %.2f, y: %.2f]", - x, y, z, d, rr, rp, ry); + ROS_DEBUG("[pick and place] Set pose target [%s] [d: %.2f]", mtk::pose2str3D(target.pose).c_str(), d); target_pose_pub_.publish(target); return true;