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_; }
/** 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;

View File

@ -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

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
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);
}

View File

@ -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;

View File

@ -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));
}

View File

@ -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;

View File

@ -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;
}

View File

@ -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?

View File

@ -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;

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 {
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();