@@ -106,8 +106,29 @@ void MoveGroupExecuteTrajectoryAction::executePath(const std::shared_ptr<ExecTra
106
106
{
107
107
RCLCPP_INFO (getLogger (), " Execution request received" );
108
108
109
+ // Convert mdof trajectory to RobotTrajectory
110
+ auto trajectory_msg = goal->get_goal ()->trajectory ;
111
+ if (!trajectory_msg.multi_dof_joint_trajectory .points .empty ())
112
+ {
113
+ std::vector<std::string> joint_names = {}; // trajectory_msg.joint_trajectory.joint_names;
114
+ for (const auto & name : trajectory_msg.multi_dof_joint_trajectory .joint_names )
115
+ {
116
+ joint_names.push_back (name);
117
+ }
118
+
119
+ robot_trajectory::RobotTrajectory trajectory (context_->moveit_cpp_ ->getRobotModel ());
120
+ trajectory.setRobotTrajectoryMsg (*context_->moveit_cpp_ ->getCurrentState (0.05 ), trajectory_msg);
121
+ const auto joint_trajectory =
122
+ robot_trajectory::toJointTrajectory (trajectory, true /* include_mdof_joints */ , joint_names);
123
+ if (joint_trajectory.has_value ())
124
+ {
125
+ trajectory_msg.joint_trajectory = joint_trajectory.value ();
126
+ trajectory_msg.multi_dof_joint_trajectory = trajectory_msgs::msg::MultiDOFJointTrajectory ();
127
+ }
128
+ }
129
+
109
130
context_->trajectory_execution_manager_ ->clear ();
110
- if (context_->trajectory_execution_manager_ ->push (goal-> get_goal ()-> trajectory , goal->get_goal ()->controller_names ))
131
+ if (context_->trajectory_execution_manager_ ->push (trajectory_msg , goal->get_goal ()->controller_names ))
111
132
{
112
133
setExecuteTrajectoryState (MONITOR, goal);
113
134
context_->trajectory_execution_manager_ ->execute ();
0 commit comments