Hello everyone!
I am working with the ROS2 driver for Humble, installed from binary.
I came across a very weird issue with the driver. I can move the robot by launching the ur_control.launch.py and the ur_moveit.launch.py, both in real life and in simulation, by sending a Plan object through a MoveGroupInterface object. However, if I send the plan through the same object instantiated within a Node object, I am not able to move the robot anymore.
More precisely, the trajectory is correctly constructed, ur_moveit.launch.py correctly receives the request and even prints out that the request was successfully executed (I can even see the phantom of the trajectory on RViz2. However, it seems like ur_moveit.launch.py is not capable to communicate the request to ur_control.launch.py if the request comes from a MoveGroupInterface within a callback.
To elaborate more, this is the script that works, though not very useful, as I cannot change the pose every time according to the case.
`
struct MyStruct {
bool success;
robot_trajectory::RobotTrajectory rt;
double fraction;
};
MyStruct plan_trajectory(std::vector<geometry_msgs::msg::Pose> &waypoints, moveit_msgs::msg::RobotTrajectory &trajectory, moveit::planning_interface::MoveGroupInterface &move_group_interface){
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, true);
// The trajectory needs to be modified so it will include velocities as well.
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(move_group_interface.getCurrentState()->getRobotModel(), "ur_manipulator");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
// Thrid create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt, 0.1);
// Get RobotTrajectory_msg from RobotTrajectory
//rt.getRobotTrajectoryMsg(trajectory);
return {success, rt, fraction};
}
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"moveit_commander",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
// Create a ROS logger
auto const logger = rclcpp::get_logger("moveit_commander");
// Create collision object for the robot to avoid
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "ur_manipulator");
// We spin up a SingleThreadedExecutor so MoveItVisualTools interact with ROS
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto spinner = std::thread([&executor]() { executor.spin(); });
move_group_interface.setEndEffectorLink("tool0");
move_group_interface.clearPoseTargets();
move_group_interface.setStartStateToCurrentState();
move_group_interface.setPlannerId("RRTkConfigDefault");
move_group_interface.setPlanningTime(15.0);
// Set a target Pose
auto const target_pose00 = []{
geometry_msgs::msg::Pose msg;
msg.position.x = -0.5296;
msg.position.y = -0.641206;
msg.position.z = 0.064395;
msg.orientation.x = 0.024036;
msg.orientation.y = 0.182602;
msg.orientation.z = 0.968574;
msg.orientation.w = -0.167162;
return msg;
}();
std::vector<geometry_msgs::msg::Pose> waypoints0;
waypoints0.push_back(target_pose00);
moveit_msgs::msg::RobotTrajectory trajectory0;
MyStruct planned_trajectory = plan_trajectory(waypoints0, trajectory0, move_group_interface);
if (planned_trajectory.success){
// Get RobotTrajectory_msg from RobotTrajectory
planned_trajectory.rt.getRobotTrajectoryMsg(trajectory0);
RCLCPP_INFO(node->get_logger(), "Executing trajectory with fraction: %f", planned_trajectory.fraction);
move_group_interface.execute(trajectory0);
}
else{
RCLCPP_ERROR(logger, "Planning failed!");
}
rclcpp::shutdown();
return 0;
}
`
- from ur_moveit.launch.py
[move_group-1] [INFO] [1707321820.725470834] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Received request to compute Cartesian path [move_group-1] [INFO] [1707321820.725528718] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Attempting to follow 1 waypoints for link 'tool0' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame) [move_group-1] [INFO] [1707321820.736252092] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Computed Cartesian path with 41 points (followed 100.000000% of requested trajectory) [move_group-1] [INFO] [1707321820.743058862] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request [move_group-1] [INFO] [1707321820.743164984] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received [move_group-1] [INFO] [1707321820.743192367] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-1] [INFO] [1707321820.743205095] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-1] [INFO] [1707321820.743254209] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 [move_group-1] [INFO] [1707321820.743611487] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ... [move_group-1] [INFO] [1707321820.743634406] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-1] [INFO] [1707321820.743647646] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-1] [INFO] [1707321820.743757039] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to joint_trajectory_controller [move_group-1] [INFO] [1707321820.744262940] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller started execution [move_group-1] [INFO] [1707321820.744279688] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! [move_group-1] [INFO] [1707321826.195569038] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'joint_trajectory_controller' successfully finished [move_group-1] [INFO] [1707321826.202276791] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... [move_group-1] [INFO] [1707321826.202532906] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: SUCCEEDED - from ur_control.launch.py
[ros2_control_node-1] [INFO] [1707321820.744082444] [joint_trajectory_controller]: Received new action goal [ros2_control_node-1] [INFO] [1707321820.744148149] [joint_trajectory_controller]: Accepted new action goal [ros2_control_node-1] [INFO] [1707321826.151682212] [joint_trajectory_controller]: Goal reached, success!
This is the script that computes everything but does not get the robot to move (I am skipping a couple of methods that are not related to the issue, but they all work):
`
class MoveItInitialiser : public rclcpp::Node {
public:
/// Constructor
MoveItInitialiser();
/// Move group interface for the robot
moveit::planning_interface::MoveGroupInterface move_group_interface;
/// Subscriber for target pose
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
/// Target pose that is used to detect changes
geometry_msgs::msg::Pose previous_target_pose_;
struct MyStruct {
bool success;
robot_trajectory::RobotTrajectory rt;
double fraction;
};
MyStruct plan_trajectory(std::vector<geometry_msgs::msg::Pose>& waypoints,
moveit_msgs::msg::RobotTrajectory& trajectory,
moveit::planning_interface::MoveGroupInterface& move_group_interface_) {
const double jump_threshold = 0.0;
const double eef_step = 0.01;
RCLCPP_INFO(this->get_logger(), "Plan initialised.");
double fraction = move_group_interface_.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, false);
// The trajectory needs to be modified so it will include velocities as well.
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(move_group_interface_.getRobotModel(), "ur_manipulator");
// Second get a RobotTrajectory from trajectory
//rt.setRobotTrajectoryMsg(*move_group_interface_.getCurrentState(), trajectory);
// Third create an IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt, 0.1);
// Get RobotTrajectory_msg from RobotTrajectory
// rt.getRobotTrajectoryMsg(trajectory);
return { success, rt, fraction };
}
private:
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg);
void load_planning_scene();
geometry_msgs::msg::PoseStamped getPrecedingFramePose(const std::string target_frame,
const geometry_msgs::msg::PoseStamped::SharedPtr target_pose);
visualization_msgs::msg::Marker waypoints_to_markers(geometry_msgs::msg::PoseStamped& waypoints);
};
geometry_msgs::msg::PoseStamped MoveItInitialiser::getPrecedingFramePose(const std::string target_frame,
const geometry_msgs::msg::PoseStamped::SharedPtr target_pose) {
// Ensure target pose has a valid timestamp
if (target_pose->header.stamp == rclcpp::Time()) {
throw std::runtime_error("Target pose must have a valid timestamp");
}
// Get the transform from the preceding frame to the target frame
geometry_msgs::msg::TransformStamped transform_stamped;
//try {
transform_stamped = tf_buffer_->lookupTransform(
"target_object", target_frame, this->now() - rclcpp::Duration::from_seconds(0.1));//, target_pose->header.stamp);
//}
//catch (const tf2::TransformException& ex) {
// throw std::runtime_error(ex.what());
//}
// Transform the target pose to the preceding frame
geometry_msgs::msg::PoseStamped preceding_frame_pose;
tf2::doTransform(*target_pose, preceding_frame_pose, transform_stamped);
return preceding_frame_pose;
}
MoveItInitialiser::MoveItInitialiser() : Node("mediated_pose_commander"),
move_group_interface(std::shared_ptr<rclcpp::Node>(std::move(this)), "ur_manipulator"){
// Subscribe to target pose
this->move_group_interface.setEndEffectorLink("tool0");
this->move_group_interface.clearPoseTargets();
this->move_group_interface.setStartStateToCurrentState();
this->move_group_interface.setPlannerId("RRTkConfigDefault");
this->move_group_interface.setPlanningTime(5.0);
target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/mediated_pose", rclcpp::QoS(1), std::bind(&MoveItInitialiser::pose_callback, this, std::placeholders::_1));
marker_publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/visual_tool0", rclcpp::QoS(1));
load_planning_scene();
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
RCLCPP_INFO(this->get_logger(), "Initialization successful.");
}
void MoveItInitialiser::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg) {
if (pose_msg->pose == previous_target_pose_)
{
RCLCPP_INFO(this->get_logger(), "Target pose has not changed. Waiting...");
return;
}
//RCLCPP_INFO(this->get_logger(), "Header timestamp: {%f}", pose_msg->header.stamp);
RCLCPP_INFO(this->get_logger(), "Position: (x: {%f}, y: {%f}, z: {%f})", pose_msg->pose.position.x, pose_msg->pose.position.y, pose_msg->pose.position.z);
RCLCPP_INFO(this->get_logger(), "Orientation: (x: {%f}, y: {%f}, z: {%f}, w: {%f})", pose_msg->pose.orientation.x, pose_msg->pose.orientation.y, pose_msg->pose.orientation.z, pose_msg->pose.orientation.w);
RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing...");
// Plan and execute trajectory based on received pose
//visualization_msgs::msg::Marker marker0 = waypoints_to_markers(*pose_msg);
//marker_publisher_->publish(marker0);
geometry_msgs::msg::PoseStamped preceding_frame_pose = getPrecedingFramePose("tool0", pose_msg);
RCLCPP_INFO(this->get_logger(), "Arrived here!");
RCLCPP_INFO(this->get_logger(), "Tool0 Position: (x: {%f}, y: {%f}, z: {%f})", preceding_frame_pose.pose.position.x, preceding_frame_pose.pose.position.y, preceding_frame_pose.pose.position.z);
RCLCPP_INFO(this->get_logger(), "Tool0 Orientation: (x: {%f}, y: {%f}, z: {%f}, w: {%f})", preceding_frame_pose.pose.orientation.x, preceding_frame_pose.pose.orientation.y, preceding_frame_pose.pose.orientation.z, preceding_frame_pose.pose.orientation.w);
// Assuming you have a function to convert your waypoints to markers, e.g., waypoints_to_markers
visualization_msgs::msg::Marker marker = waypoints_to_markers(preceding_frame_pose);
visualization_msgs::msg::MarkerArray marker_array;
marker_array.markers.push_back(marker);
// Publish the MarkerArray
marker_publisher_->publish(marker_array);
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(preceding_frame_pose.pose);
moveit_msgs::msg::RobotTrajectory trajectory;
MyStruct planned_trajectory = plan_trajectory(waypoints, trajectory, this->move_group_interface);
RCLCPP_INFO(this->get_logger(), "Fraction: %f", planned_trajectory.fraction);
if (planned_trajectory.success) {
// Get RobotTrajectory_msg from RobotTrajectory
planned_trajectory.rt.getRobotTrajectoryMsg(trajectory);
RCLCPP_INFO(this->get_logger(), "Planning successful!");
this->move_group_interface.execute(trajectory);
} else {
RCLCPP_ERROR(this->get_logger(), "Planning failed!");
}
previous_target_pose_ = pose_msg->pose;
}
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
auto target_follower = std::make_shared<MoveItInitialiser>();
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(target_follower);
executor.spin();
rclcpp::shutdown();
return EXIT_SUCCESS;
}
`
For this code, when run together with the launch files, I get the following output on the launch files terminal
- from ur_moveit.launch.py
[move_group-1] [INFO] [1707320209.387487881] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Received request to compute Cartesian path [move_group-1] [INFO] [1707320209.387608013] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Attempting to follow 1 waypoints for link 'tool0' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame) [move_group-1] [INFO] [1707320209.392100213] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Computed Cartesian path with 41 points (followed 100.000000% of requested trajectory) [move_group-1] [INFO] [1707320209.393030660] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request [move_group-1] [INFO] [1707320209.393173201] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received [move_group-1] [INFO] [1707320209.393210687] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 [move_group-1] [INFO] [1707320209.394122668] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ... [move_group-1] [INFO] [1707320209.394160938] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-1] [INFO] [1707320209.394183785] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-1] [INFO] [1707320209.399613870] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... [move_group-1] [INFO] [1707320209.399658325] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: SUCCEEDED
- from ur_control_launch.py, the terminal does not print anything new
Strangely, when the request comes from a callbcack, ur_moveit.launch.py does not print the statements in bold in the functioning case:
[move_group-1] [INFO] [1707321820.743757039] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to joint_trajectory_controller
[move_group-1] [INFO] [1707321820.744262940] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller started execution
[move_group-1] [INFO] [1707321820.744279688] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-1] [INFO] [1707321826.195569038] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller ‘joint_trajectory_controller’ successfully finished
I do not know why, maybe it is something related to simple_controller_manager or moveit_plugins, I am not sure. I am always sending the same pose that I have stored in a bag file.
Sadly, I need this last script to work, as I am controlling the robot position through poses I send in real time that I compute in other scripts. So, I need this script to receive poses from a callback to then send it to MoveIt2 through the MoveGroupInterface within the node. I am pretty sure I am doing something dumb, but it looks like the driver behaves differently if the MoveGroupInterface is in the main or in the object.
Both scripts are compiled with no errors togethre with the rest of the workspace they share. Nothing fails during the execution. Changing from Single to MultiThreadExecutor seems not to do anything, nor populating a Plan object with the Trajectory to send rather than sending the Trajectory itself, but it is worth checking again.
Any solutions/turnarounds?
In fewer words, I need to figure out a working way to send trajectories/plans for the driver to perform that from a callback rather than from the main.
Thanks!