Found empty JointState message trying to use MotionSequenceRequest (Pilz Industrial Motion Planner)

[NilsHeidemann]

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?