add external properties to SolutionInfo

[v4hn]

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]

@felixvd

[codecov]

Codecov Report

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

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

[rhaschke]

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]

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

[rhaschke]

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

[rhaschke]

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

[rhaschke]

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

[v4hn]

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

[gautz]

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?

[rhaschke]

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

[rhaschke]

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

[rhaschke]

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

[rhaschke]

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]

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]

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

[captain-yoshi]

Suggested change

	void fromMsgs(std::vector<moveit_task_constructor_msgs::Property>& msgs);
	void fromMsgs(const std::vector<moveit_task_constructor_msgs::Property>& msgs);

[captain-yoshi]

Suggested change

	void PropertyMap::fromMsgs(std::vector<moveit_task_constructor_msgs::Property>& msgs) {
	void PropertyMap::fromMsgs(const std::vector<moveit_task_constructor_msgs::Property>& msgs) {

[captain-yoshi]

Compile error on Noetic:

Compilation error:
In file included from /opt/ros/noetic/include/rviz/properties/vector_property.h:32,
                 from /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/visualization/motion_planning_tasks/properties/property_factory.cpp:46:
/opt/ros/noetic/include/rviz/ogre_helpers/ogre_vector.h:2:10: fatal error: OgrePrerequisites.h: No such file or directory
    2 | #include <OgrePrerequisites.h>
    ```

[captain-yoshi]

Trying to recreate a MoveRelative from the Property.msg throws an error:

stage->properties().fromMsgs(property_msg);

terminate called after throwing an instance of 'moveit::task_constructor::Property::type_error'
  what():  Property: type v doesn't match property's declared type St3setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4lessIS5_ESaIS5_EE

(gdb) bt
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff65b2859 in __GI_abort () at abort.c:79
#2  0x00007ffff683b8d1 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff684737c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff68473e7 in std::terminate() () from /lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff6847699 in __cxa_throw () from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff78327de in moveit::task_constructor::PropertyMap::declare (this=0x55557e80bb80, name="forwarded_properties", type_info=..., description="set of interface properties to forward", 
    default_value=...) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/properties.cpp:204
#7  0x00007ffff7832bdb in moveit::task_constructor::PropertyMap::fromMsgs (this=0x55557e80bb80, msgs=std::vector of length 12, capacity 12 = {...})
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/properties.cpp:232

The culprit is the forwarded_properties which is of type std::set<std::string>. This type should be added to the serialization.

Test to add:
1- Create a MoveRelative stage1
2- stage->fillMsgs(property_msg)
3- Create another MoveRelative stage2 (should also work on stage1)
4- stage2->fromMsgs(property_msg)

Output of Property.msg

name: controller_id
type: NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE
value: joint_position_controller
description:

name: direction
type: geometry_msgs/Vector3Stamped
value: pcr����Mb@�
description: motion specification

name: direction_offset
type: geometry_msgs/Pose
value: <�O��nr?��MbX�?-C��6J?�;f���?�;f���?
description: frame offset wrt. the direction frame_id

name: forwarded_properties
type: St3setINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESt4lessIS5_ESaIS5_EE
value: 
description: set of interface properties to forward

name: group
type: NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE
value: eef_pipette
description: name of planning group

name: ik_frame
type: geometry_msgs/PoseStamped
value: 
        pipette_tcp�?
description: frame to be moved in Cartesian direction

name: marker_ns
type: NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE
setPlanningSceneDiffMsg: value = Move Relative Above Bottom Container
description: marker namespace

name: max_distance
type: d
value: 0
description: maximum distance to move

name: min_distance
type: d
value: -1
description: minimum distance to move

name: path_constraints
type: moveit_msgs/Constraints
value: 
description: constraints to maintain during trajectory

name: timeout
type: d
value: 1
description: timeout per run (s)

name: trajectory_execution_info
type: moveit_task_constructor_msgs/TrajectoryExecutionInfo
value: 
description: settings used when executing the trajectory

[captain-yoshi]

I have added tests and fixes in this branch. This now works for my use case.

I would also prefer the YAML serialization over binary. You can even have exact saving/loading of floating point numbers, at the cost of size increase.