search

splintered-reality / py_trees_ros_tutorials

Tutorials for py_trees on ros
Other
10 stars 8 forks source link

/joint_states subscriber to blackboard #50

Open slyandsmart opened 6 days ago

slyandsmart commented 6 days ago

I try to build up a little BH-Tree for my mobile robot with a small manipulator. I wanted to substribe /joint_states topic within the Tree to write it to the blackboard.

This works quite fine with artificial topic from simulation: jointstates_to_blackboard = py_trees_ros.subscribers.ToBlackboard( name="jointstates_to_blackboard", topic_type=JointState, topic_name="/joint_simulation/JointState", qos_profile=py_trees_ros.utilities.qos_profile_unlatched(), blackboard_variables={'jointstate': None} )

but it dos not work using standard /joint_states topic jointstates_to_blackboard = py_trees_ros.subscribers.ToBlackboard( name="jointstates_to_blackboard", topic_type=JointState, topic_name="/joint_states", qos_profile=py_trees_ros.utilities.qos_profile_unlatched(), blackboard_variables={'jointstate': None} )

Brings up the error:

[rcutils|error_handling.c:108] rcutils_set_error_state() This error state is being overwritten:

'create_subscription() called for existing topic name rt/joint_states with incompatible type sensormsgs::msg::dds::JointState_, at ./src/subscription.cpp:146, at ./src/rcl/subscription.c:109'

with this new error message:

'invalid allocator, at ./src/rcl/subscription.c:219'

rcutils_reset_error() should be called after error handling to avoid this. <<< invalid allocator, at ./src/rcl/subscription.c:219 Traceback (most recent call last): File "/workspace/src/emt_acm_do/do_slot_dozing/do_slot_dozing/blade_move_behaviour_tree.py", line 252, in main() File "/workspace/src/emt_acm_do/do_slot_dozing/do_slot_dozing/blade_move_behaviour_tree.py", line 225, in main tree.setup(timeout=15) File "/opt/ros/humble/lib/python3.10/site-packages/py_trees_ros/trees.py", line 437, in setup super().setup( File "/opt/ros/humble/lib/python3.10/site-packages/py_trees/trees.py", line 378, in setup setup(root=self.root, timeout=timeout, visitor=visitor, kwargs) File "/opt/ros/humble/lib/python3.10/site-packages/py_trees/trees.py", line 131, in setup visited_setup() File "/opt/ros/humble/lib/python3.10/site-packages/py_trees/trees.py", line 110, in visited_setup node.setup(kwargs) File "/opt/ros/humble/lib/python3.10/site-packages/py_trees_ros/subscribers.py", line 115, in setup self.subscriber = self.node.create_subscription( File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/node.py", line 1370, in create_subscription subscription_object = _rclpy.Subscription( rclpy._rclpy_pybind11.RCLError: Failed to create subscription: invalid allocator, at ./src/rcl/subscription.c:219

With any other topic it works fine.

slyandsmart commented 6 days ago

I think i found the problem image

I have also a subscribers.EventToBlackboard set up, but with this implementation it adds the topic type as shown in picture.

I think this will not work as i understand it correct?