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;