Merge branches 'analyze-property-errors' and 'fixes'

This commit is contained in:
Robert Haschke 2018-02-24 13:45:51 +01:00
commit a24039846a
10 changed files with 87 additions and 27 deletions

View File

@ -73,7 +73,7 @@ public:
inline const container_type& children() const { return children_; } inline const container_type& children() const { return children_; }
/** Retrieve iterator into children_ pointing to indexed element. /** 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. */ * Contrary to std::advance(), iterator limits are considered. */
const_iterator position(int index) const; const_iterator position(int index) const;

View File

@ -65,7 +65,7 @@ ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) c
container_type::const_reverse_iterator from_end = children_.rbegin(); container_type::const_reverse_iterator from_end = children_.rbegin();
for (auto end = children_.rend(); index < 0 && from_end != end; ++index) for (auto end = children_.rend(); index < 0 && from_end != end; ++index)
++from_end; ++from_end;
position = from_end.base(); position = index < 0 ? children_.end() : from_end.base();
} }
return position; return position;
} }
@ -511,23 +511,31 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
// determine accepted interface from available push interfaces // determine accepted interface from available push interfaces
InterfaceFlags accepted; InterfaceFlags accepted;
// if previous stage pushes forward, we accept forward propagation // if first stage ...
if (first != children().begin()) { if (first != children().begin()) {
auto prev = first; --prev; // pointer to previous stage auto prev = first; --prev; // pointer to previous stage
// ... pushes forward, we accept forward propagation
if ((*prev)->pimpl()->requiredInterface() & WRITES_NEXT_START) 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 } // 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()) { if (end != children().end()) {
// ... pushes backward, we accept backward propagation
if ((*end)->pimpl()->requiredInterface() & WRITES_PREV_END) 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 } // else: for last child we cannot determine the interface yet
// nothing to do if: // nothing to do if:
// - accepted == 0: interface still unknown // - accepted == 0: interface still unknown
// - accepted == PROPAGATE_FORWARDS | PROPAGATE_BACKWARDS: no change // - 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); pruneInterfaces(first, end, accepted);
} }
@ -540,13 +548,13 @@ void SerialContainerPrivate::pruneInterfaces(container_type::const_iterator firs
for (auto it = first; it != end; ++it) { for (auto it = first; it != end; ++it) {
StagePrivate* impl = (*it)->pimpl(); StagePrivate* impl = (*it)->pimpl();
// range should only contain stages with unknown required interface // range should only contain stages with unknown required interface
assert(impl->requiredInterface() == PropagatingEitherWay::AUTO); assert(impl->requiredInterface() == UNKNOWN);
// remove push interfaces // remove push interfaces
if (!(accepted & WRITES_PREV_END)) if (!(accepted & PROPAGATE_BACKWARDS))
impl->setPrevEnds(InterfacePtr()); impl->setPrevEnds(InterfacePtr());
if (!(accepted & WRITES_NEXT_START)) if (!(accepted & PROPAGATE_FORWARDS))
impl->setNextStarts(InterfacePtr()); impl->setNextStarts(InterfacePtr());
} }
// 2nd sweep: recursively prune children // 2nd sweep: recursively prune children

View File

@ -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 // 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::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 <> 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::CollisionSharedPtr& element(const urdf::Link& link) { return link.collision; }
template <> const urdf::VisualSharedPtr& element(const urdf::Link& link) { return link.visual; } 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; if (!link) return;
bool valid_found = false; bool valid_found = false;
auto element_handler = [&](const boost::shared_ptr<T>& element){ auto element_handler = [&](const T& element){
if (element && element->geometry) { if (element && element->geometry) {
createGeometryMarker(m, *element->geometry, element->origin, createGeometryMarker(m, *element->geometry, element->origin,
materialColor(*model, materialName(*element))); materialColor(*model, materialName(*element)));
@ -123,30 +123,30 @@ void generateMarkers(const moveit::core::RobotState &robot_state,
void generateCollisionMarkers(const moveit::core::RobotState &robot_state, void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback, const MarkerCallback& callback,
const std::vector<std::string> &link_names) { 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, void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback &callback, const MarkerCallback &callback,
const std::vector<const moveit::core::LinkModel*> &link_models) { 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, void generateVisualMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback, const MarkerCallback& callback,
const std::vector<std::string> &link_names) { 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, void generateVisualMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback &callback, const MarkerCallback &callback,
const std::vector<const moveit::core::LinkModel*> &link_models) { 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 /** generate marker msgs to visualize the planning scene, calling the given callback for each of them
* calls generateMarkersForRobot() and generateMarkersForObjects() */ * calls generateMarkersForRobot() and generateMarkersForObjects() */
void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr &scene, void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr &scene,
const MarkerCallback &callback) { const MarkerCallback &callback) {
generateMarkers<urdf::Visual>(scene->getCurrentState(), callback); generateMarkers<urdf::VisualSharedPtr>(scene->getCurrentState(), callback);
generateMarkersForObjects(scene, callback); generateMarkersForObjects(scene, callback);
} }

View File

@ -62,7 +62,7 @@ const char *InitStageException::what() const noexcept
} }
std::ostream& operator<<(std::ostream& os, const InitStageException& e) { 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_) for (const auto &pair : e.errors_)
os << pair.first->name() << ": " << pair.second << std::endl; os << pair.first->name() << ": " << pair.second << std::endl;
return os; return os;

View File

@ -242,6 +242,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
// set scene's robot state // set scene's robot state
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data()); robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
robot_state.update();
spawn(InterfaceState(scene), std::move(solution)); spawn(InterfaceState(scene), std::move(solution));
} }

View File

@ -105,9 +105,7 @@ bool GenerateGraspPose::compute(){
robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst(); robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst();
const std::string& eef_named_pose = props.get<std::string>("eef_named_pose"); 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"); geometry_msgs::TransformStamped tool2grasp_msg = props.get<geometry_msgs::TransformStamped>("tool_to_grasp_tf");
const std::string &link_name = jmg ->getEndEffectorParentGroup().second; const std::string &link_name = jmg ->getEndEffectorParentGroup().second;

View File

@ -110,8 +110,6 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
{ {
planning_scene::PlanningScenePtr scene = from.scene()->diff(); planning_scene::PlanningScenePtr scene = from.scene()->diff();
InterfaceState result(scene); InterfaceState result(scene);
// initialize properties from input state
result.properties() = from.properties();
// attach/detach objects // attach/detach objects
for (const auto &pair : attach_objects_) for (const auto &pair : attach_objects_)
@ -122,7 +120,7 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
enableCollisions(*scene, pairs, invert); enableCollisions(*scene, pairs, invert);
if (callback_) if (callback_)
callback_(scene, result.properties()); callback_(scene, properties());
return result; return result;
} }

View File

@ -150,7 +150,8 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
linear_norm = linear.norm(); linear_norm = linear.norm();
angular_norm = angular.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_rotation_distance = linear_norm < std::numeric_limits<double>::epsilon();
// use max distance? // use max distance?

View File

@ -64,7 +64,7 @@ Interface::iterator Interface::add(InterfaceState &&state, SolutionBase* incomin
if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty()) if (!state.incomingTrajectories().empty() || !state.outgoingTrajectories().empty())
throw std::runtime_error("expecting empty incoming/outgoing trajectories"); throw std::runtime_error("expecting empty incoming/outgoing trajectories");
if (!state.scene()) 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 // move state to a list node
std::list<InterfaceState> container; std::list<InterfaceState> container;

View File

@ -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 { class SerialTest : public ::testing::Test {
protected: protected:
moveit::core::RobotModelConstPtr robot_model; moveit::core::RobotModelConstPtr robot_model;
@ -258,6 +292,26 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS)); EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(serial.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE)); 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 // derive propagation direction from outer interface
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> -> EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
it = serial.pimpl()->children().begin(); it = serial.pimpl()->children().begin();