Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add external properties to SolutionInfo #194

Open
wants to merge 12 commits into
base: master
Choose a base branch
from

Conversation

v4hn
Copy link
Contributor

@v4hn v4hn commented Jul 30, 2020

This interface can be customized by the user to state when/where additional execution hooks should run.

This is an initial thread to discuss/extend this idea as discussed in #192

@v4hn v4hn requested a review from rhaschke July 30, 2020 18:09
@v4hn
Copy link
Contributor Author

v4hn commented Jul 30, 2020

@felixvd

@codecov
Copy link

codecov bot commented Jul 30, 2020

Codecov Report

Merging #194 (45935c0) into master (06b3df9) will increase coverage by 0.26%.
The diff coverage is 75.60%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master     #194      +/-   ##
==========================================
+ Coverage   51.63%   51.88%   +0.26%     
==========================================
  Files         101      102       +1     
  Lines        6590     7202     +612     
==========================================
+ Hits         3402     3736     +334     
- Misses       3188     3466     +278     
Impacted Files Coverage Δ
...e/include/moveit/task_constructor/stages/connect.h 100.00% <ø> (ø)
core/src/stages/fixed_cartesian_poses.cpp 0.00% <0.00%> (ø)
...ion_planning_tasks/properties/property_factory.cpp 0.00% <0.00%> (ø)
core/src/properties.cpp 85.98% <56.25%> (+0.67%) ⬆️
core/include/moveit/task_constructor/properties.h 91.67% <84.62%> (+11.67%) ⬆️
core/src/stages/connect.cpp 88.47% <85.72%> (-3.04%) ⬇️
...e/moveit/task_constructor/properties/serialize.hpp 94.55% <94.55%> (ø)
core/src/introspection.cpp 73.69% <100.00%> (-2.38%) ⬇️
core/include/moveit/task_constructor/task.h 33.34% <0.00%> (-16.66%) ⬇️
core/src/solvers/joint_interpolation.cpp 74.20% <0.00%> (-7.28%) ⬇️
... and 50 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 06b3df9...45935c0. Read the comment docs.

msgs/msg/SolutionInfo.msg Outdated Show resolved Hide resolved
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note that a Property's value field is a string comprising a serialized version of an arbitrary property.
The serialization/deserialization methods are not (yet) exposed and might be subject to changes. There are also some properties, which don't (yet) have a serialization/deserialization method defined.
Hence, the Property type might not be the best choice to implement the behavior here. At least we would need to expose the deserialization methods...

@v4hn
Copy link
Contributor Author

v4hn commented Aug 11, 2020

Note that a Property's value field is a string comprising a serialized version of an arbitrary property.

That's exactly why I would prefer it over some string indicator fields for this.

The serialization/deserialization methods are not (yet) exposed and might be subject to changes.

Same for everything else in MTC.

Hence, the Property type might not be the best choice to implement the behavior here.
At least we would need to expose the deserialization methods...

From a pragmatic point of view it works just fine with std::string properties, because their serialization is the string itself. In general though, we definitely want to expose deserialization functions if we want users to actually use these properties. Currently, the deserialization is mixed in with the rviz/visualization code...

@v4hn v4hn force-pushed the pr-external-properties branch from a3f816a to 89f9d2f Compare September 6, 2021 13:30
v4hn added 5 commits September 6, 2021 15:32
Better usability for user code if they deserialize individual Propertys.

The fallback variants are not used internally and are redundant
because there is a separate default behavior.

Removed the msg parameter for Property exceptions.
It was only used by a single edge-case which didn't really justify
the use of the exception anyway.
Instead allow `undefined` to be instantiated without parameters
because the Property does not know its own name.
Defaults are not supposed to be modified by accident.
Instead, provide a writable reference to currentValue.
@v4hn v4hn force-pushed the pr-external-properties branch from 89f9d2f to 50ef6ea Compare September 6, 2021 13:33
@v4hn v4hn force-pushed the pr-external-properties branch from 50ef6ea to fb0fa19 Compare September 9, 2021 20:34
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just a few remarks... We definitely should have a phone call to sync a little bit on our activities 😉

core/src/properties.cpp Outdated Show resolved Hide resolved
@rhaschke
Copy link
Contributor

I was long thinking about why I decided against your proposed solution when I worked on Property serialization.
Eventually, I remembered and found the corresponding PR #72:
For serialization/deserialization to work, they need to access registered methods. This is simple on the serialization end, because we know the types to serialize:
https://github.com/ros-planning/moveit_task_constructor/blob/0aff5d56dd45b91b7c6c7c63c9674eec84e49aed/core/include/moveit/task_constructor/properties.h#L266-L270
However, on the deserialization end, we typically don't know the type in advance. Thus I decided to go for the generic YAML output generated by operator<< for ROS messages. This has two advantages:

  • The serialized output is human-readable, e.g. when intercepting rostopic.
  • rviz, the main deserialization site, can use a generic deserializer to display values, even if no type-specific one is registered. Using ROS message serialization we lose this capability: Note, how ik_frame is displayed before and after this PR:
old new
old new

@rhaschke
Copy link
Contributor

We discussed pros and cons of the old and the proposed approach in a phone conference. This is the summary:

old, YAML-based serialization new, binary serialization
pro human-readable, e.g. with rostopic exact saving/loading of floating point numbers
deserialization for rviz existing C++ YAML parser python deserialize ➔ python YAML ➔ existing C++ YAML parser
msg deserialization YAML ➔ python msg ➔ python serialize ➔ C++ deserialize ROS deserialize
open issues YAML from ROS msgs partly broken (requires fix in gencpp or workarounds) will require uint8[] representation

Copy link
Contributor Author

@v4hn v4hn left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

thanks for the summary. here are some more comments I wrote before the talk reviewing your changes.

@gautz
Copy link

gautz commented Jan 11, 2022

I want to execute subsolutions of a solution sequentially and in between set IOs or call services.
Should I use the solution from #192 or does it make already sense to try to use this PR?

Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure about the status of this PR. @v4hn, maybe we should schedule another phone call?

Comment on lines +281 to +283
} catch (Property::undefined& e) {
e.setName(name);
throw e;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Catching and rethrowing is more costly than the previous version of testing and throwing only once.

/// the current value defined or will the fallback be used?
inline bool defined() const { return !value_.empty(); }
/// is a value defined?
inline bool defined() const { return !(value_.empty() && default_.empty()); }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here you change the semantics of defined()!
I guess you want to distinguish isDefined() from isSet() == hasCurrentValue()?
Let's discuss the naming over phone...

const boost::any& v{ value() };
if (v.empty())
throw Property::undefined();
return boost::any_cast<const T&>(value());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
return boost::any_cast<const T&>(value());
return boost::any_cast<const T&>(v);

See also my comment on PropertyMap::get().

@captain-yoshi
Copy link
Contributor

What is the status of this PR ? Would be really useful. I think a lot of people got their own way of dealing with this.


BTW this PR needs this patch,

void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const {
    info.id = introspection ? introspection->solutionId(*this) : 0;                                                
    info.cost = this->cost();                                                                                      
    info.comment = this->comment();                                                                                
    const Introspection* ci = introspection;                                                                       
    info.stage_id = ci ? ci->stageId(this->creator()) : 0;                                                         
    
    // got to add this
    creator_->properties().fillMsgs(info.properties);
                                                              
    const auto& markers = this->markers();                                                                         
    info.markers.resize(markers.size());                                                                           
    std::copy(markers.begin(), markers.end(), info.markers.begin());                                               
}                                                                                                                 

to actually print the properties.

  for (const auto& solution : task->solutions())
  {
    moveit_task_constructor_msgs::Solution solution_msg;
    solution->fillMessage(solution_msg, &task->introspection());

    for (const auto& sub_traj : solution_msg.sub_trajectory)
    {
      std::cout << "==========" << std::endl;
      std::cout << "stage_id: " << sub_traj.info.id << std::endl;

      auto props = sub_traj.info.properties;

      for (moveit_task_constructor_msgs::Property& p : props)
      {
        std::cout << "name: " << p.name << std::endl;
        std::cout << "type: " << p.type << std::endl;
        std::cout << "val : " << p.value << std::endl;
        std::cout << "---------" << std::endl;
      }
    }
  }


==========
stage_id: 1
name: forwarded_properties
type: St3setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4lessIS5_ESaIS5_EE
val : 
---------
name: hey
type: NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE
val : 
---------
name: ignore_collisions
type: b
val : 0
---------
name: marker_ns
type: NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE
val : current state
---------
name: timeout
type: d
val : 
---------

...

@captain-yoshi
Copy link
Contributor

@v4hn Trying to serialize the ros msg control_msgs::GripperCommand. Do you have an idea why it does not serialize ? Other ros msg are ok...

control_msgs::GripperCommand controller_goal;
controller_goal.max_effort = 100;
controller_goal.position = robot_state.joint_state.position.front();

stage->setProperty("point", point);
stage->setProperty("controller_goal", controller_goal);

// Prints
name: point
type: geometry_msgs/Vector3Stamped
val : thermal_cycler���Q��?
--------
name: controller_goal
type: 
val : 

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants