search

moveit / moveit_task_constructor

A hierarchical multi-stage manipulation planner
https://moveit.github.io/moveit_task_constructor
BSD 3-Clause "New" or "Revised" License
184 stars 152 forks source link

Unregistered property type #527

Open captain-yoshi opened 10 months ago

captain-yoshi commented 10 months ago

When adding this ros msg to a stage:

control_msgs::GripperCommand controller_goal;

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

I get this warning:

[ WARN] [1706843311.825944146]: Unregistered property type: control_msgs::GripperCommand_<std::allocator<void> >

Do you have an idea on why this triggers ? I though that all ros msgs could be serialized...

rhaschke commented 10 months ago

Yes, in principle all ROS msgs can be serialized. But they have to be explicitly registered (for the deserialization system). I guess that message originates from rviz not being able to deserialize the GripperCommand msg?

captain-yoshi commented 9 months ago

I think it originates from mtc.

Thread 1 "close_lid_biora" hit Breakpoint 1, moveit::task_constructor::PropertyTypeRegistry::entry (this=0x7ffff7ca3d40 <moveit::task_constructor::REGISTRY_SINGLETON>, type_index=...)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/properties.cpp:76
76              ROS_WARN_STREAM_THROTTLE_NAMED(
(gdb) bt
#0  moveit::task_constructor::PropertyTypeRegistry::entry (this=0x7ffff7ca3d40 <moveit::task_constructor::REGISTRY_SINGLETON>, type_index=...)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/properties.cpp:76
#1  0x00007ffff7b0aa7b in moveit::task_constructor::Property::typeName[abi:cxx11](std::type_info const&) (type_info=...)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/properties.cpp:174
#2  0x00007ffff7b0a9cc in moveit::task_constructor::Property::typeName[abi:cxx11]() const (this=0x555555e8b3e0) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/properties.cpp:167
#3  0x00007ffff7ad6446 in moveit::task_constructor::Introspection::<lambda(const moveit::task_constructor::Stage&, unsigned int)>::operator()(const moveit::task_constructor::Stage &, unsigned int) const (
    __closure=0x7fffffff9940, stage=...) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/introspection.cpp:239
#4  0x00007ffff7ad6b75 in std::_Function_handler<bool(const moveit::task_constructor::Stage&, unsigned int), moveit::task_constructor::Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription&)::<lambda(const moveit::task_constructor::Stage&, unsigned int)> >::_M_invoke(const std::_Any_data &, const moveit::task_constructor::Stage &, unsigned int &&) (__functor=..., __args#0=..., 
    __args#1=@0x7fffffff987c: 1) at /usr/include/c++/9/bits/std_function.h:285
#5  0x00007ffff7a897c8 in std::function<bool (moveit::task_constructor::Stage const&, unsigned int)>::operator()(moveit::task_constructor::Stage const&, unsigned int) const (this=0x7fffffff9940, __args#0=..., 
    __args#1=1) at /usr/include/c++/9/bits/std_function.h:688
#6  0x00007ffff7a75fcd in moveit::task_constructor::ContainerBasePrivate::traverseStages(std::function<bool (moveit::task_constructor::Stage const&, unsigned int)> const&, unsigned int, unsigned int) const (
    this=0x55555569e260, processor=..., cur_depth=1, max_depth=4294967295) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:141
#7  0x00007ffff7a76d9f in moveit::task_constructor::ContainerBase::traverseRecursively(std::function<bool (moveit::task_constructor::Stage const&, unsigned int)> const&) const (this=0x5555556962e0, 
    processor=...) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/container.cpp:356
#8  0x00007ffff7ad66ef in moveit::task_constructor::Introspection::fillTaskDescription (this=0x55555569f5b0, msg=...)
    at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/introspection.cpp:254
#9  0x00007ffff7ad5999 in moveit::task_constructor::Introspection::publishTaskDescription (this=0x55555569f5b0) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/introspection.cpp:139
#10 0x00007ffff7b31d41 in moveit::task_constructor::Task::init (this=0x7fffffff9cb0) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/task.cpp:244
#11 0x00007ffff7b31ec7 in moveit::task_constructor::Task::plan (this=0x7fffffff9cb0, max_solutions=5) at /home/captain-yoshi/ws/ros/mimik_ws/src/moveit_task_constructor/core/src/task.cpp:257
#12 0x00005555555a64bd in main ()
(gdb) thread apply all where

Thread 8 (Thread 0x7fffed67c700 (LWP 35787)):
#0  futex_abstimed_wait_cancelable (private=<optimized out>, abstime=0x7fffed67b930, clockid=<optimized out>, expected=0, futex_word=0x555555693230) at ../sysdeps/nptl/futex-internal.h:320
#1  __pthread_cond_wait_common (abstime=0x7fffed67b930, clockid=<optimized out>, mutex=0x5555556931e0, cond=0x555555693208) at pthread_cond_wait.c:520
#2  __pthread_cond_timedwait (cond=0x555555693208, mutex=0x5555556931e0, abstime=0x7fffed67b930) at pthread_cond_wait.c:665
#3  0x00007ffff71d3baf in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/noetic/lib/libroscpp.so
#4  0x00005555555be659 in actionlib::SimpleActionClient<control_msgs::Robotiq2fGripperCommandAction_<std::allocator<void> > >::spinThread() ()
#5  0x00007ffff6f8b43b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#6  0x00007ffff7059609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#7  0x00007ffff6b3a353 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Thread 7 (Thread 0x7fffede7d700 (LWP 35786)):
#0  futex_abstimed_wait_cancelable (private=<optimized out>, abstime=0x7fffede7c930, clockid=<optimized out>, expected=0, futex_word=0x55555568f540) at ../sysdeps/nptl/futex-internal.h:320
#1  __pthread_cond_wait_common (abstime=0x7fffede7c930, clockid=<optimized out>, mutex=0x55555568f4f0, cond=0x55555568f518) at pthread_cond_wait.c:520
#2  __pthread_cond_timedwait (cond=0x55555568f518, mutex=0x55555568f4f0, abstime=0x7fffede7c930) at pthread_cond_wait.c:665
#3  0x00007ffff71d3baf in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/noetic/lib/libroscpp.so
#4  0x00005555555be569 in actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction_<std::allocator<void> > >::spinThread() ()
#5  0x00007ffff6f8b43b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#6  0x00007ffff7059609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#7  0x00007ffff6b3a353 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Thread 6 (Thread 0x7fffee67e700 (LWP 35785)):
#0  futex_abstimed_wait_cancelable (private=<optimized out>, abstime=0x7fffee67d930, clockid=<optimized out>, expected=0, futex_word=0x5555556827e8) at ../sysdeps/nptl/futex-internal.h:320
#1  __pthread_cond_wait_common (abstime=0x7fffee67d930, clockid=<optimized out>, mutex=0x555555682798, cond=0x5555556827c0) at pthread_cond_wait.c:520
#2  __pthread_cond_timedwait (cond=0x5555556827c0, mutex=0x555555682798, abstime=0x7fffee67d930) at pthread_cond_wait.c:665
#3  0x00007ffff71d3baf in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/noetic/lib/libroscpp.so
#4  0x00007ffff722631e in ros::AsyncSpinnerImpl::threadFunc() () from /opt/ros/noetic/lib/libroscpp.so
#5  0x00007ffff6f8b43b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#6  0x00007ffff7059609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#7  0x00007ffff6b3a353 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Thread 5 (Thread 0x7fffeee7f700 (LWP 35784)):
#0  futex_abstimed_wait_cancelable (private=<optimized out>, abstime=0x7fffeee7e920, clockid=<optimized out>, expected=0, futex_word=0x55555568ca48) at ../sysdeps/nptl/futex-internal.h:320
#1  __pthread_cond_wait_common (abstime=0x7fffeee7e920, clockid=<optimized out>, mutex=0x55555568c9f8, cond=0x55555568ca20) at pthread_cond_wait.c:520
#2  __pthread_cond_timedwait (cond=0x55555568ca20, mutex=0x55555568c9f8, abstime=0x7fffeee7e920) at pthread_cond_wait.c:665
#3  0x00007ffff71d3baf in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/noetic/lib/libroscpp.so
#4  0x00007ffff720efbf in ros::internalCallbackQueueThreadFunc() () from /opt/ros/noetic/lib/libroscpp.so
#5  0x00007ffff6f8b43b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#6  0x00007ffff7059609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#7  0x00007ffff6b3a353 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Thread 4 (Thread 0x7fffef680700 (LWP 35783)):
#0  futex_wait_cancelable (private=<optimized out>, expected=0, futex_word=0x555555688c2c) at ../sysdeps/nptl/futex-internal.h:183
#1  __pthread_cond_wait_common (abstime=0x0, clockid=0, mutex=0x555555688bd8, cond=0x555555688c00) at pthread_cond_wait.c:508
#2  __pthread_cond_wait (cond=0x555555688c00, mutex=0x555555688bd8) at pthread_cond_wait.c:647
#3  0x00007ffff720c7d7 in ros::ROSOutAppender::logThread() () from /opt/ros/noetic/lib/libroscpp.so
#4  0x00007ffff6f8b43b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#5  0x00007ffff7059609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#6  0x00007ffff6b3a353 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Thread 3 (Thread 0x7fffefe81700 (LWP 35782)):
#0  0x00007ffff6b2dbbf in __GI___poll (fds=0x7fffe0001130, nfds=3, timeout=100) at ../sysdeps/unix/sysv/linux/poll.c:29
#1  0x00007ffff68634dd in XmlRpc::XmlRpcDispatch::work(double) () from /opt/ros/noetic/lib/libxmlrpcpp.so
#2  0x00007ffff71a4f0a in ros::XMLRPCManager::serverThreadFunc() () from /opt/ros/noetic/lib/libroscpp.so
#3  0x00007ffff6f8b43b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#4  0x00007ffff7059609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#5  0x00007ffff6b3a353 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
rhaschke commented 9 months ago

Yes, you are right. Probably this can and should be improved... Let's see when someone finds time to do so... The underlying problem is that properties are not templated, but essentially use boost::any to become generic.

rhaschke commented 8 months ago

You missed to declare the property: stage->properties().declare<control_msgs::GripperCommand>("controller_goal");

Alternatively, you could use the following to declare (and set the default) in one go: stage->properties().set("controller_goal", controller_goal);

Stage::setProperty is currently not templated and thus we cannot declare/register the type there. Maybe we should change this. Opinions, @v4hn?

captain-yoshi commented 6 months ago

I also get unregistered property type warning

[ WARN] [1716837715.989871855]: Unregistered property type: std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > >

when setting a MoveRelative with a joint.

auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("move rel", solver);

std::map<std::string, double> joints{{"panda_joint7", 1.57}};
stage->setDirection(joints);
...
rhaschke commented 6 months ago

As this is not a ROS message, we don't have a (de)serialization method for it.