Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces
#1636
Conversation
…time --> added gitignore, to not push it to github --> remove it after everything is done
… the hardware interface can receive a new motion primitive — replaces the previous, more complex handling via execution_status
github-actions
bot
requested review from
ARK3r,
bmagyar,
christophfroehlich,
fmauch,
mcbed,
peterdavidfagan and
VX792
There was a problem hiding this comment.
There is a lot of stuff coming from templates in this PR and you either used some AI tool for coding or you are very good at writing AI-style comments.
Please try removing all commented code or fix them and remove all comments that add nothing to the code itself (which in this code is almost all comments...)
...mitives_forward_controller/include/motion_primitives_forward_controller/visibility_control.h
Outdated
Show resolved
Hide resolved
motion_primitives_forward_controller/motion_primitives_forward_controller.rolling.repos
Outdated
Show resolved
Hide resolved
motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp
Outdated
Show resolved
Hide resolved
...roller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp
Outdated
Show resolved
Hide resolved
…ls according to other controllers in the repo
… rtw template to fit the controller implementation
…tation from rtw template to fit the controller implementation
…ontroller namespace
|
Thanks @bmagyar for your review! I've addressed and resolved all the comments. Looking forward to any further feedback! |
motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp
Outdated
Show resolved
Hide resolved
|
It would also be great if the actions and messages from |
| "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>" | ||
| "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>") | ||
| target_link_libraries(motion_primitives_forward_controller motion_primitives_forward_controller_parameters) | ||
| ament_target_dependencies(motion_primitives_forward_controller |
There was a problem hiding this comment.
ament_target_dependencies is deprecated and was even temporarily removed from Rolling. See ament/ament_cmake#584 (comment) for details.
|
|
||
| install( | ||
| DIRECTORY include/ | ||
| DESTINATION include |
There was a problem hiding this comment.
| DESTINATION include | |
| DESTINATION include/${PROJECT_NAME} |
| target_include_directories( | ||
| test_load_motion_primitives_forward_controller PRIVATE include | ||
| ) |
There was a problem hiding this comment.
Wouldn't it make more sense to add motion_primitives_forward_controller to the target_link_libraries` instead of manually adding the include dir?
| target_include_directories( | ||
| test_load_motion_primitives_forward_controller PRIVATE include | ||
| ) | ||
| ament_target_dependencies( |
| target_link_libraries( | ||
| test_motion_primitives_forward_controller motion_primitives_forward_controller | ||
| ) | ||
| ament_target_dependencies( |
| // Check if queue has enough space | ||
| if (moprim_queue_.size() + required_queue_space > queue_size_) | ||
| { | ||
| RCLCPP_ERROR( | ||
| get_node()->get_logger(), | ||
| "Received more motion primitives than queue can store. " | ||
| "Current queue size: %zu, required space: %zu. " | ||
| "Please increase the queue size in the controller parameters.", | ||
| moprim_queue_.size(), required_queue_space); | ||
| return; | ||
| } |
There was a problem hiding this comment.
As mentioned above, I don't quite see the necessity of restricting this.
| for (const auto & primitive : primitives) | ||
| { | ||
| moprim_queue_.push(std::make_shared<MotionPrimitiveMsg>(primitive)); | ||
| } |
There was a problem hiding this comment.
You could wrap that into a lambda and then only have one conditional block:
auto add_motions = [this](primitives){
for (const auto & primitive : primitives)
{
moprim_queue_.push(std::make_shared<MotionPrimitiveMsg>(primitive));
}
};
if (primitives.size() > 1)
{
std::shared_ptr<MotionPrimitiveMsg> start_marker = std::make_shared<MotionPrimitiveMsg>();
start_marker->type = MotionType::MOTION_SEQUENCE_START;
moprim_queue_.push(start_marker);
add_motions(primitives);
std::shared_ptr<MotionPrimitiveMsg> end_marker = std::make_shared<MotionPrimitiveMsg>();
end_marker->type = MotionType::MOTION_SEQUENCE_END;
moprim_queue_.push(end_marker);
}
else
{
add_motions(primitives);
}| rclcpp::Rate rate(50); | ||
| while (rclcpp::ok()) | ||
| { | ||
| auto execution_status = static_cast<uint8_t>(std::round(state_interfaces_[0].get_value())); |
There was a problem hiding this comment.
You are basically accessing a shared resource (state_interfaces_ gets set inside the control thread) without thread synchronization. One more point against using the execute thread.
| } | ||
| uint8_t ready_for_new_primitive = static_cast<int8_t>(std::round(opt_value_ready.value())); | ||
|
|
||
| if (!moprim_queue_.empty()) // check if new command is available |
There was a problem hiding this comment.
Access to shared resource without thread synchronization. I am not sure whether empty() is a thread-safe call, but I wouldn't expect it.
| } | ||
| case ReadyForNewPrimitive::READY: | ||
| { | ||
| if (!set_command_interfaces()) |
There was a problem hiding this comment.
This is where you potentially block the control thread as this will wait for aquiring the lock for accessing the queue.
This PR adds the
motion_primitives_forward_controller, a controller for forwarding motion primitive commands to motion-primitive-capable hardware interfaces.It was developed alongside the
motion_primitive_ur_driverand is intended to support future hardware interfaces for additional robot manufacturers.The corresponding PR for integrating the
motion_primitive_ur_driverinto the official Universal Robots ROS 2 driver can be found here: UniversalRobots/Universal_Robots_ROS2_Driver#1341.The controller subscribes to
MotionPrimitive.msgfrom theindustrial_robot_motion_interfacespackage. TheMotionPrimitive.msghas been extended with additional helper types:STOP_MOTION: Immediately interrupts execution and clears all queued primitives.MOTION_SEQUENCE_STARTandMOTION_SEQUENCE_END: Define a sequence of primitives to be executed as a blended motion block.The controller uses the following interfaces:
Command Interfaces
motion_type: Type of motion primitive (e.g., LINEAR_JOINT, LINEAR_CARTESIAN, CIRCULAR_CARTESIAN, etc.)q1–q6: Target joint positions for joint-based motionpos_x,pos_y,pos_z: Target Cartesian positionpos_qx,pos_qy,pos_qz,pos_qw: Orientation quaternion of the target posepos_via_x,pos_via_y,pos_via_z: Intermediate via-point position for circular motionpos_via_qx,pos_via_qy,pos_via_qz,pos_via_qw: Orientation quaternion of via-pointblend_radius: Blending radius for smooth transitionsvelocity: Desired motion velocityacceleration: Desired motion accelerationmove_time: Optional duration for time-based executionState Interfaces
execution_status: Indicates the current execution state of the primitive.ready_for_new_primitive: Boolean flag indicating whether the interface is ready to receive a new motion primitiveI'd appreciate any feedback or suggestions – thanks in advance!