mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
address interface changes for object poses in MoveIt
Also include a check for the new object pose field in `Connecting::compatible()`.
This commit is contained in:
parent
0273aec83e
commit
f51f6eb982
@ -38,6 +38,8 @@
|
|||||||
#include <moveit/task_constructor/stage_p.h>
|
#include <moveit/task_constructor/stage_p.h>
|
||||||
#include <moveit/task_constructor/container_p.h>
|
#include <moveit/task_constructor/container_p.h>
|
||||||
#include <moveit/task_constructor/introspection.h>
|
#include <moveit/task_constructor/introspection.h>
|
||||||
|
#include <moveit/task_constructor/moveit_compat.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
@ -811,14 +813,23 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
|||||||
|
|
||||||
// both scenes should have the same set of collision objects, at the same location
|
// both scenes should have the same set of collision objects, at the same location
|
||||||
for (const auto& from_object_pair : *from->getWorld()) {
|
for (const auto& from_object_pair : *from->getWorld()) {
|
||||||
|
const std::string& from_object_name = from_object_pair.first;
|
||||||
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
|
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
|
||||||
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first);
|
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name);
|
||||||
if (!to_object) {
|
if (!to_object) {
|
||||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_pair.first);
|
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_name);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if MOVEIT_HAS_OBJECT_POSE
|
||||||
|
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) {
|
||||||
|
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name);
|
||||||
|
return false; // transforms do not match
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
|
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
|
||||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_pair.first);
|
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name);
|
||||||
return false; // shapes not matching
|
return false; // shapes not matching
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -826,7 +837,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
|||||||
to_it = to_object->shape_poses_.cbegin();
|
to_it = to_object->shape_poses_.cbegin();
|
||||||
from_it != from_end; ++from_it, ++to_it)
|
from_it != from_end; ++from_it, ++to_it)
|
||||||
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
|
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
|
||||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_pair.first);
|
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different shape pose: " << from_object_name);
|
||||||
return false; // transforms do not match
|
return false; // transforms do not match
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -857,16 +868,24 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
|||||||
<< to_object->getAttachedLinkName());
|
<< to_object->getAttachedLinkName());
|
||||||
return false; // links not matching
|
return false; // links not matching
|
||||||
}
|
}
|
||||||
if (from_object->getFixedTransforms().size() != to_object->getFixedTransforms().size()) {
|
if (from_object->getShapes().size() != to_object->getShapes().size()) {
|
||||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName());
|
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName());
|
||||||
return false; // shapes not matching
|
return false; // shapes not matching
|
||||||
}
|
}
|
||||||
|
|
||||||
for (auto from_it = from_object->getFixedTransforms().cbegin(),
|
#if MOVEIT_HAS_OBJECT_POSE
|
||||||
from_end = from_object->getFixedTransforms().cend(), to_it = to_object->getFixedTransforms().cbegin();
|
auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
|
||||||
from_it != from_end; ++from_it, ++to_it)
|
auto from_end = from_object->getShapePosesInLinkFrame().cend();
|
||||||
|
auto to_it = to_object->getShapePosesInLinkFrame().cbegin();
|
||||||
|
#else
|
||||||
|
auto from_it = from_object->getFixedTransforms().cbegin();
|
||||||
|
auto from_end = from_object->getFixedTransforms().cend();
|
||||||
|
auto to_it = to_object->getFixedTransforms().cbegin();
|
||||||
|
#endif
|
||||||
|
for (; from_it != from_end; ++from_it, ++to_it)
|
||||||
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
|
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
|
||||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object->getName());
|
ROS_DEBUG_STREAM_NAMED("Connecting",
|
||||||
|
name() << ": different pose of attached object shape: " << from_object->getName());
|
||||||
return false; // transforms do not match
|
return false; // transforms do not match
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -37,6 +37,7 @@
|
|||||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||||
#include <moveit/task_constructor/storage.h>
|
#include <moveit/task_constructor/storage.h>
|
||||||
#include <moveit/task_constructor/marker_tools.h>
|
#include <moveit/task_constructor/marker_tools.h>
|
||||||
|
#include <moveit/task_constructor/moveit_compat.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
@ -295,7 +296,12 @@ void ComputeIK::compute() {
|
|||||||
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
|
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
const EigenSTL::vector_Isometry3d& tf = attached->getFixedTransforms();
|
const EigenSTL::vector_Isometry3d& tf =
|
||||||
|
#if MOVEIT_HAS_OBJECT_POSE
|
||||||
|
attached->getShapePosesInLinkFrame();
|
||||||
|
#else
|
||||||
|
attached->getFixedTransforms();
|
||||||
|
#endif
|
||||||
if (tf.empty()) {
|
if (tf.empty()) {
|
||||||
ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes.");
|
ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes.");
|
||||||
return;
|
return;
|
||||||
|
|||||||
@ -37,7 +37,9 @@
|
|||||||
#include <moveit/task_constructor/stages/generate_place_pose.h>
|
#include <moveit/task_constructor/stages/generate_place_pose.h>
|
||||||
#include <moveit/task_constructor/storage.h>
|
#include <moveit/task_constructor/storage.h>
|
||||||
#include <moveit/task_constructor/marker_tools.h>
|
#include <moveit/task_constructor/marker_tools.h>
|
||||||
|
|
||||||
#include <rviz_marker_tools/marker_creation.h>
|
#include <rviz_marker_tools/marker_creation.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/robot_state.h>
|
#include <moveit/robot_state/robot_state.h>
|
||||||
#include <moveit/robot_state/attached_body.h>
|
#include <moveit/robot_state/attached_body.h>
|
||||||
@ -63,7 +65,7 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
|
|||||||
std::string msg;
|
std::string msg;
|
||||||
if (!scene->getCurrentState().hasAttachedBody(object))
|
if (!scene->getCurrentState().hasAttachedBody(object))
|
||||||
msg = "'" + object + "' is not an attached object";
|
msg = "'" + object + "' is not an attached object";
|
||||||
if (scene->getCurrentState().getAttachedBody(object)->getFixedTransforms().empty())
|
if (scene->getCurrentState().getAttachedBody(object)->getShapes().empty())
|
||||||
msg = "'" + object + "' has no associated shapes";
|
msg = "'" + object + "' has no associated shapes";
|
||||||
if (!msg.empty()) {
|
if (!msg.empty()) {
|
||||||
if (storeFailures()) {
|
if (storeFailures()) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user