ros-planning
Found empty JointState message trying to use MotionSequenceRequest (Pilz Industrial Motion Planner)
Hi all,
I am trying to implement sequence executions using the Pilz_Industrial_Motion_Planner. I have followed your documentation to write the following script for sequence execution:
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/string.hpp"
#include <rclcpp_action/rclcpp_action.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/msg/motion_sequence_item.hpp>
#include <moveit_msgs/msg/motion_sequence_request.hpp>
#include <moveit_msgs/msg/planning_options.hpp>
#include <moveit_msgs/srv/get_motion_sequence.hpp>
#include <moveit_msgs/action/move_group_sequence.hpp>
using moveit_msgs::action::MoveGroupSequence;
using GoalHandleMoveGroupSequence = rclcpp_action::ClientGoalHandle<MoveGroupSequence>;
// Create a ROS logger
auto const LOGGER = rclcpp::get_logger("pilz_sequence_node");
// Callback-Funktion, die ausgeführt wird, wenn eine Nachricht empfangen wird
void topic_callback(const std_msgs::msg::String::SharedPtr msg)
{
RCLCPP_INFO(LOGGER, "Empfangene Nachricht: '%s'", msg->data.c_str());
// Hier kannst du die empfangenen Daten verarbeiten
}
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
int main(int argc, char** argv)
{
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto node = rclcpp::Node::make_shared("pilz_sequence_node", node_options);
// We spin up a SingleThreadedExecutor so MoveItVisualTools interact with ROS
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
std::thread([&executor]() { executor.spin(); }).detach();
// Subscriber hinzufügen
auto subscription = node->create_subscription<std_msgs::msg::String>(
"joint_states", // Name des Topics, das abonniert werden soll
10, // Nachrichten-Queue-Größe
topic_callback // Callback-Funktion, die ausgeführt wird
);
// Planning group
static const std::string PLANNING_GROUP = "manipulator";
// Create the MoveIt MoveGroup Interface
// In this case, this is just necessary for the visual interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, PLANNING_GROUP);
// [ --------------------------------------------------------------- ]
// [ ----------------------- Motion Sequence ----------------------- ]
// [ --------------------------------------------------------------- ]
// ----- Motion Sequence Item 1
// Create a MotionSequenceItem
moveit_msgs::msg::MotionSequenceItem item1;
// Set pose blend radius
item1.blend_radius = 0.1;
// MotionSequenceItem configuration
item1.req.group_name = PLANNING_GROUP;
item1.req.planner_id = "PTP";
item1.req.allowed_planning_time = 5.0;
item1.req.max_velocity_scaling_factor = 0.1;
item1.req.max_acceleration_scaling_factor = 0.1;
moveit_msgs::msg::Constraints constraints_item1;
moveit_msgs::msg::PositionConstraint pos_constraint_item1;
pos_constraint_item1.header.frame_id = "base_link";
pos_constraint_item1.link_name = "tcp";
// Set a constraint pose
auto target_pose_item1 = [] {
geometry_msgs::msg::PoseStamped msg;
msg.header.frame_id = "base_link";
msg.pose.position.x = 0.3;
msg.pose.position.y = -0.2;
msg.pose.position.z = 0.6;
msg.pose.orientation.x = 1.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
msg.pose.orientation.w = 0.0;
return msg;
}();
item1.req.goal_constraints.push_back(
kinematic_constraints::constructGoalConstraints("J6_link", target_pose_item1));
// ----- Motion Sequence Item 2
// Create a MotionSequenceItem
moveit_msgs::msg::MotionSequenceItem item2;
// Set pose blend radius
// For the last pose, it must be 0!
item2.blend_radius = 0.0;
// MotionSequenceItem configuration
item2.req.group_name = PLANNING_GROUP;
item2.req.planner_id = "PTP";
item2.req.allowed_planning_time = 5.0;
item2.req.max_velocity_scaling_factor = 0.1;
item2.req.max_acceleration_scaling_factor = 0.1;
// Set a constraint pose
auto target_pose_item2 = [] {
geometry_msgs::msg::PoseStamped msg;
msg.header.frame_id = "base_link";
msg.pose.position.x = 0.3;
msg.pose.position.y = -0.2;
msg.pose.position.z = 0.8;
msg.pose.orientation.x = 1.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
msg.pose.orientation.w = 0.0;
return msg;
}();
item2.req.goal_constraints.push_back(
kinematic_constraints::constructGoalConstraints("J6_link", target_pose_item2));
// [ --------------------------------------------------------------- ]
// [ ------------------ MoveGroupSequence Action ------------------- ]
// [ --------------------------------------------------------------- ]
// Plans and executes the trajectory
// MoveGroupSequence action client
using MoveGroupSequence = moveit_msgs::action::MoveGroupSequence;
auto action_client = rclcpp_action::create_client<MoveGroupSequence>(node, "/sequence_move_group");
// Verify that the action server is up and running
if (!action_client->wait_for_action_server(std::chrono::seconds(10)))
{
RCLCPP_ERROR(LOGGER, "Error waiting for action server /sequence_move_group");
return -1;
}
// Create a MotionSequenceRequest
moveit_msgs::msg::MotionSequenceRequest sequence_request;
sequence_request.items.push_back(item1);
sequence_request.items.push_back(item2);
// Create action goal
auto goal_msg = MoveGroupSequence::Goal();
goal_msg.request = sequence_request;
// Planning options
goal_msg.planning_options.planning_scene_diff.is_diff = true;
goal_msg.planning_options.planning_scene_diff.robot_state.is_diff = true;
// goal_msg.planning_options.plan_only = true; // Uncomment to only plan the trajectory
// Goal response callback
auto send_goal_options = rclcpp_action::Client<MoveGroupSequence>::SendGoalOptions();
send_goal_options.goal_response_callback = [](std::shared_ptr<GoalHandleMoveGroupSequence> goal_handle) {
try
{
if (!goal_handle)
{
RCLCPP_ERROR(LOGGER, "Goal was rejected by server");
}
else
{
RCLCPP_INFO(LOGGER, "Goal accepted by server, waiting for result");
}
}
catch (const std::exception& e)
{
RCLCPP_ERROR(LOGGER, "Exception while waiting for goal response: %s", e.what());
}
};
// Result callback
send_goal_options.result_callback = [](const GoalHandleMoveGroupSequence::WrappedResult& result) {
switch (result.code)
{
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(LOGGER, "Goal succeeded");
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(LOGGER, "Goal was aborted. Status: %d", result.result->response.error_code.val);
break;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(LOGGER, "Goal was canceled");
break;
default:
RCLCPP_ERROR(LOGGER, "Unknown result code");
break;
}
RCLCPP_INFO(LOGGER, "Result received");
};
// Send the action goal
auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
// Get result
auto action_result_future = action_client->async_get_result(goal_handle_future.get());
// Wait for the result
std::future_status action_status;
do
{
switch (action_status = action_result_future.wait_for(std::chrono::seconds(1)); action_status)
{
case std::future_status::deferred:
RCLCPP_ERROR(LOGGER, "Deferred");
break;
case std::future_status::timeout:
RCLCPP_INFO(LOGGER, "Executing trajectory...");
break;
case std::future_status::ready:
RCLCPP_INFO(LOGGER, "Action ready!");
break;
}
} while (action_status != std::future_status::ready);
if (action_result_future.valid())
{
auto result = action_result_future.get();
RCLCPP_INFO(LOGGER, "Action completed. Result: %d", static_cast<int>(result.code));
}
else
{
RCLCPP_ERROR(LOGGER, "Action couldn't be completed.");
}
rclcpp::shutdown();
return 0;
}
If I use PTP as planner_id I get the following console output:
[move_group-5] [INFO] [1738255619.675127098] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: Combined planning and execution request received for MoveGroupSequenceAction. [move_group-5] [INFO] [1738255619.675176138] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1 [move_group-5] [ERROR] [1738255619.675312183] [moveit_robot_state.conversions]: Found empty JointState message [move_group-5] [ERROR] [1738255619.675339440] [moveit_robot_state.conversions]: Found empty JointState message [move_group-5] [ERROR] [1738255619.675384029] [moveit_robot_state.conversions]: Found empty JointState message [move_group-5] [INFO] [1738255619.675436191] [moveit.pilz_industrial_motion_planner.trajectory_generator_ptp]: Initialized Point-to-Point Trajectory Generator. [move_group-5] [INFO] [1738255619.675473769] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating PTP trajectory... [move_group-5] [INFO] [1738255619.714980071] [moveit.pilz_industrial_motion_planner.trajectory_generator_ptp]: Initialized Point-to-Point Trajectory Generator. [move_group-5] [INFO] [1738255619.714999294] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating PTP trajectory... [move_group-5] [INFO] [1738255619.721037006] [moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window]: Start trajectory blending using transition window. [move_group-5] [ERROR] [1738255619.721051182] [moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window]: During blending the last point of the preceding and the first point of the succeeding trajectory [move_group-5] [ERROR] [1738255619.721053402] [moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window]: Trajectory blend request is not valid. [move_group-5] [ERROR] [1738255619.721074517] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: Planning pipeline threw an exception (error code: 99999): Blending failed [move_group-5] [WARN] [1738255619.721085124] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: Can not determine start state from empty sequence.
If I use LIN as planner_id I get the following console output:
[move_group-5] [INFO] [1738256199.660069940] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: Combined planning and execution request received for MoveGroupSequenceAction. [move_group-5] [INFO] [1738256199.660139829] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1 [move_group-5] [ERROR] [1738256199.660285387] [moveit_robot_state.conversions]: Found empty JointState message [move_group-5] [ERROR] [1738256199.660316364] [moveit_robot_state.conversions]: Found empty JointState message [move_group-5] [ERROR] [1738256199.660369136] [moveit_robot_state.conversions]: Found empty JointState message [move_group-5] [INFO] [1738256199.660443516] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory... [move_group-5] [ERROR] [1738256199.678137307] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Unable to find IK solution. [move_group-5] [ERROR] [1738256199.678164557] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Failed to compute inverse kinematics solution for sampled Cartesian pose. [move_group-5] [ERROR] [1738256199.678236261] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Failed to generate valid joint trajectory from the Cartesian path [move_group-5] [ERROR] [1738256199.678277322] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: Planning pipeline threw an exception (error code: -31): Could not solve request [move_group-5] [move_group-5] [WARN] [1738256199.678299495] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: Can not determine start state from empty sequence.
I guess the Found empty JointState message is the reason its not working. I tried
ros2 topic echo /joint_states
and it is not empty:
Does someone have an idea how to solve this issue?
