mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'analyze-property-errors' and 'fixes'
This commit is contained in:
commit
a24039846a
@ -73,7 +73,7 @@ public:
|
||||
inline const container_type& children() const { return children_; }
|
||||
|
||||
/** Retrieve iterator into children_ pointing to indexed element.
|
||||
* Negative index counts from end().
|
||||
* Negative index counts from end(), i.e. -1 is end(), -2 is --end(), etc.
|
||||
* Contrary to std::advance(), iterator limits are considered. */
|
||||
const_iterator position(int index) const;
|
||||
|
||||
|
||||
@ -65,7 +65,7 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) c
|
||||
container_type::const_reverse_iterator from_end = children_.rbegin();
|
||||
for (auto end = children_.rend(); index < 0 && from_end != end; ++index)
|
||||
++from_end;
|
||||
position = from_end.base();
|
||||
position = index < 0 ? children_.end() : from_end.base();
|
||||
}
|
||||
return position;
|
||||
}
|
||||
@ -511,23 +511,31 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
|
||||
// determine accepted interface from available push interfaces
|
||||
InterfaceFlags accepted;
|
||||
|
||||
// if previous stage pushes forward, we accept forward propagation
|
||||
// if first stage ...
|
||||
if (first != children().begin()) {
|
||||
auto prev = first; --prev; // pointer to previous stage
|
||||
// ... pushes forward, we accept forward propagation
|
||||
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START)
|
||||
accepted |= WRITES_NEXT_START;
|
||||
accepted |= PROPAGATE_FORWARDS;
|
||||
// ... pulls backward, we accept backward propagation
|
||||
if ((*prev)->pimpl()->requiredInterface() & READS_END)
|
||||
accepted |= PROPAGATE_BACKWARDS;
|
||||
} // else: for first child we cannot determine the interface yet
|
||||
|
||||
// if end stage pushes backward, we accept backward propagation
|
||||
// if end stage ...
|
||||
if (end != children().end()) {
|
||||
// ... pushes backward, we accept backward propagation
|
||||
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END)
|
||||
accepted |= WRITES_PREV_END;
|
||||
accepted |= PROPAGATE_BACKWARDS;
|
||||
// ... pulls forward, we accept forward propagation
|
||||
if ((*end)->pimpl()->requiredInterface() & READS_START)
|
||||
accepted |= PROPAGATE_FORWARDS;
|
||||
} // else: for last child we cannot determine the interface yet
|
||||
|
||||
// nothing to do if:
|
||||
// - accepted == 0: interface still unknown
|
||||
// - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change
|
||||
if (accepted != 0 && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}))
|
||||
if (accepted != UNKNOWN && accepted != InterfaceFlags({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS}))
|
||||
pruneInterfaces(first, end, accepted);
|
||||
}
|
||||
|
||||
@ -540,13 +548,13 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
|
||||
for (auto it = first; it != end; ++it) {
|
||||
StagePrivate* impl = (*it)->pimpl();
|
||||
// range should only contain stages with unknown required interface
|
||||
assert(impl->requiredInterface() == PropagatingEitherWay::AUTO);
|
||||
assert(impl->requiredInterface() == UNKNOWN);
|
||||
|
||||
// remove push interfaces
|
||||
if (!(accepted & WRITES_PREV_END))
|
||||
if (!(accepted & PROPAGATE_BACKWARDS))
|
||||
impl->setPrevEnds(InterfacePtr());
|
||||
|
||||
if (!(accepted & WRITES_NEXT_START))
|
||||
if (!(accepted & PROPAGATE_FORWARDS))
|
||||
impl->setNextStarts(InterfacePtr());
|
||||
}
|
||||
// 2nd sweep: recursively prune children
|
||||
|
||||
@ -58,11 +58,11 @@ const urdf::Color& materialColor(const urdf::ModelInterface& model, const std::s
|
||||
}
|
||||
|
||||
// type traits to access collision/visual array or single element
|
||||
template <class T> const std::vector<boost::shared_ptr<T>>& elements_vector(const urdf::Link& link);
|
||||
template <class T> const std::vector<T>& elements_vector(const urdf::Link& link);
|
||||
template <> const std::vector<urdf::CollisionSharedPtr>& elements_vector(const urdf::Link& link) { return link.collision_array; }
|
||||
template <> const std::vector<urdf::VisualSharedPtr>& elements_vector(const urdf::Link& link) { return link.visual_array; }
|
||||
|
||||
template <class T> const boost::shared_ptr<T>& element(const urdf::Link& link);
|
||||
template <class T> const T& element(const urdf::Link& link);
|
||||
template <> const urdf::CollisionSharedPtr& element(const urdf::Link& link) { return link.collision; }
|
||||
template <> const urdf::VisualSharedPtr& element(const urdf::Link& link) { return link.visual; }
|
||||
|
||||
@ -100,7 +100,7 @@ void generateMarkers(const moveit::core::RobotState &robot_state,
|
||||
if (!link) return;
|
||||
|
||||
bool valid_found = false;
|
||||
auto element_handler = [&](const boost::shared_ptr<T>& element){
|
||||
auto element_handler = [&](const T& element){
|
||||
if (element && element->geometry) {
|
||||
createGeometryMarker(m, *element->geometry, element->origin,
|
||||
materialColor(*model, materialName(*element)));
|
||||
@ -123,30 +123,30 @@ void generateMarkers(const moveit::core::RobotState &robot_state,
|
||||
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback& callback,
|
||||
const std::vector<std::string> &link_names) {
|
||||
generateMarkers<urdf::Collision>(robot_state, callback, link_names);
|
||||
generateMarkers<urdf::CollisionSharedPtr>(robot_state, callback, link_names);
|
||||
}
|
||||
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback &callback,
|
||||
const std::vector<const moveit::core::LinkModel*> &link_models) {
|
||||
generateMarkers<urdf::Collision>(robot_state, callback, linkNames(link_models));
|
||||
generateMarkers<urdf::CollisionSharedPtr>(robot_state, callback, linkNames(link_models));
|
||||
}
|
||||
|
||||
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback& callback,
|
||||
const std::vector<std::string> &link_names) {
|
||||
generateMarkers<urdf::Visual>(robot_state, callback, link_names);
|
||||
generateMarkers<urdf::VisualSharedPtr>(robot_state, callback, link_names);
|
||||
}
|
||||
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
|
||||
const MarkerCallback &callback,
|
||||
const std::vector<const moveit::core::LinkModel*> &link_models) {
|
||||
generateMarkers<urdf::Visual>(robot_state, callback, linkNames(link_models));
|
||||
generateMarkers<urdf::VisualSharedPtr>(robot_state, callback, linkNames(link_models));
|
||||
}
|
||||
|
||||
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them
|
||||
* calls generateMarkersForRobot() and generateMarkersForObjects() */
|
||||
void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr &scene,
|
||||
const MarkerCallback &callback) {
|
||||
generateMarkers<urdf::Visual>(scene->getCurrentState(), callback);
|
||||
generateMarkers<urdf::VisualSharedPtr>(scene->getCurrentState(), callback);
|
||||
generateMarkersForObjects(scene, callback);
|
||||
}
|
||||
|
||||
|
||||
@ -62,7 +62,7 @@ const char *InitStageException::what() const noexcept
|
||||
}
|
||||
|
||||
std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
|
||||
os << e.what() << std::endl;
|
||||
os << "Error initializing stage" << (e.errors_.size() > 1 ? "s" : "") << ":" << std::endl;
|
||||
for (const auto &pair : e.errors_)
|
||||
os << pair.first->name() << ": " << pair.second << std::endl;
|
||||
return os;
|
||||
|
||||
@ -242,6 +242,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
// set scene's robot state
|
||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
|
||||
robot_state.update();
|
||||
|
||||
spawn(InterfaceState(scene), std::move(solution));
|
||||
}
|
||||
|
||||
@ -105,9 +105,7 @@ bool GenerateGraspPose::compute(){
|
||||
|
||||
robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst();
|
||||
const std::string& eef_named_pose = props.get<std::string>("eef_named_pose");
|
||||
if(!eef_named_pose.empty()){
|
||||
robot_state.setToDefaultValues(jmg , eef_named_pose);
|
||||
}
|
||||
robot_state.setToDefaultValues(jmg , eef_named_pose);
|
||||
|
||||
geometry_msgs::TransformStamped tool2grasp_msg = props.get<geometry_msgs::TransformStamped>("tool_to_grasp_tf");
|
||||
const std::string &link_name = jmg ->getEndEffectorParentGroup().second;
|
||||
|
||||
@ -110,8 +110,6 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
|
||||
{
|
||||
planning_scene::PlanningScenePtr scene = from.scene()->diff();
|
||||
InterfaceState result(scene);
|
||||
// initialize properties from input state
|
||||
result.properties() = from.properties();
|
||||
|
||||
// attach/detach objects
|
||||
for (const auto &pair : attach_objects_)
|
||||
@ -122,7 +120,7 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
|
||||
enableCollisions(*scene, pairs, invert);
|
||||
|
||||
if (callback_)
|
||||
callback_(scene, result.properties());
|
||||
callback_(scene, properties());
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@ -150,7 +150,8 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
|
||||
linear_norm = linear.norm();
|
||||
angular_norm = angular.norm();
|
||||
angular /= angular_norm; // normalize angular
|
||||
if (angular_norm > std::numeric_limits<double>::epsilon())
|
||||
angular /= angular_norm; // normalize angular
|
||||
use_rotation_distance = linear_norm < std::numeric_limits<double>::epsilon();
|
||||
|
||||
// use max distance?
|
||||
|
||||
@ -64,7 +64,7 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
|
||||
if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty())
|
||||
throw std::runtime_error("expecting empty incoming/outgoing trajectories");
|
||||
if (!state.scene())
|
||||
throw std::runtime_error("expecting valid planning scene");
|
||||
throw std::runtime_error("expecting valid planning scene in InterfaceState");
|
||||
|
||||
// move state to a list node
|
||||
std::list<InterfaceState> container;
|
||||
|
||||
@ -66,6 +66,40 @@ void append(ContainerBase& c, const std::initializer_list<StageType>& types, int
|
||||
}
|
||||
|
||||
|
||||
class NamedStage : public GeneratorMockup {
|
||||
public:
|
||||
NamedStage(const std::string& name) : GeneratorMockup() {
|
||||
setName(name);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(ContainerBase, position) {
|
||||
SerialContainer s;
|
||||
SerialContainerPrivate *impl = s.pimpl();
|
||||
|
||||
EXPECT_EQ(impl->position(0), impl->children().end());
|
||||
EXPECT_EQ(impl->position(1), impl->children().end());
|
||||
EXPECT_EQ(impl->position(-1), impl->children().end());
|
||||
EXPECT_EQ(impl->position(-2), impl->children().end());
|
||||
|
||||
s.insert(std::make_unique<NamedStage>("0"));
|
||||
EXPECT_STREQ((*impl->position(0))->name().c_str(), "0");
|
||||
EXPECT_EQ(impl->position(-1), impl->children().end());
|
||||
EXPECT_STREQ((*impl->position(-2))->name().c_str(), "0");
|
||||
EXPECT_EQ(impl->position(-3), impl->children().end());
|
||||
|
||||
s.insert(std::make_unique<NamedStage>("1"));
|
||||
EXPECT_STREQ((*impl->position(0))->name().c_str(), "0");
|
||||
EXPECT_STREQ((*impl->position(1))->name().c_str(), "1");
|
||||
EXPECT_EQ(impl->position(2), impl->children().end());
|
||||
|
||||
EXPECT_EQ(impl->position(-1), impl->children().end());
|
||||
EXPECT_STREQ((*impl->position(-2))->name().c_str(), "1");
|
||||
EXPECT_STREQ((*impl->position(-3))->name().c_str(), "0");
|
||||
EXPECT_EQ(impl->position(-4), impl->children().end());
|
||||
}
|
||||
|
||||
|
||||
class SerialTest : public ::testing::Test {
|
||||
protected:
|
||||
moveit::core::RobotModelConstPtr robot_model;
|
||||
@ -258,6 +292,26 @@ TEST_F(SerialTest, interface_detection) {
|
||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, true, GEN, ANY);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||
|
||||
EXPECT_INIT_SUCCESS(true, true, ANY, GEN);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
|
||||
|
||||
// derive propagation direction from inner connector
|
||||
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
|
||||
it = serial.pimpl()->children().begin();
|
||||
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
|
||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||
|
||||
EXPECT_INIT_SUCCESS(false, false, CONN, ANY);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||
|
||||
EXPECT_INIT_SUCCESS(false, false, ANY, CONN);
|
||||
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
|
||||
|
||||
// derive propagation direction from outer interface
|
||||
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
|
||||
it = serial.pimpl()->children().begin();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user