eprosima::fastcdr::exception::NotEnoughMemoryException with rclpy.ActionClient
I'm using ROS 2 Foxy on Ubuntu 20.04.
Something wacky is going on with my Durations.
#!/usr/bin/env python3
from control_msgs.action import FollowJointTrajectory
import rclpy
from rclpy.action import ActionClient
from rclpy.duration import Duration
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectoryPoint
class TrajectoryActionClient(Node):
def __init__(self):
super().__init__('trajectory_action_client')
self._action_client = ActionClient(self, FollowJointTrajectory,
'/stretch_controller/follow_joint_trajectory')
goal = FollowJointTrajectory.Goal()
goal.trajectory.joint_names = ['wrist_extension']
p0 = JointTrajectoryPoint()
p0.positions = [0.1]
p0.velocities = [0.0]
p0.accelerations = [0.0]
goal.trajectory.points.append(p0)
p1 = JointTrajectoryPoint()
p1.positions = [0.1]
p1.velocities = [0.0]
p1.accelerations = [0.0]
p1.time_from_start = Duration(seconds=5).to_msg()
goal.trajectory.points.append(p1)
self._send_goal_future = self._action_client.send_goal_async(goal)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
return
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = TrajectoryActionClient()
rclpy.spin(node)
if __name__ == '__main__':
main()
If I run this code as is, I get the following:
terminate called after throwing an instance of 'eprosima::fastcdr::exception::NotEnoughMemoryException'
what(): Not enough memory in the buffer stream
But if I comment out the line
p1.time_from_start = Duration(seconds=5).to_msg()
It runs fine. Changing it to
p1.time_from_start.sec = 5
also doesn't work.
Hey "good news" the same error happens with rclcpp!
I have created an isolated package example here: https://github.com/DLu/my_happy_package
Note: The error only occurs when you run on two machines.
Can you try it with cyclone? that would at least tell us if it is a middleware issue or perhaps something in rclpy. Also, do you think you could produce a backtrace for the exception?
For switching the rmw implementation, see: https://docs.ros.org/en/foxy/Guides/W...
For getting a C stack trace and/or python traceback from a python script (using gdb): https://wiki.python.org/moin/Debuggin...
It seems (according to your package's README), that it works with rolling? It could be a bug in FastDDS that was fixed but not back ported, but it doesn't remind me of anything I know about.
In which case... it might be a good time to escalate this to an issue on https://github.com/ros2/rmw_fastrtps/
I confirm that it works fine with cyclone, which explains why it works fine in galactic and rolling.
I have put a stack trace here: https://github.com/DLu/my_happy_packa...
https://github.com/ros2/rmw_fastrtps/...