Skip to content

Commit 03e4b8a

Browse files
svillalorensvillaloren
committed
Adding debug prints inside move action callback
1 parent 3f2d7f5 commit 03e4b8a

1 file changed

Lines changed: 120 additions & 3 deletions

File tree

‎moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp‎

Lines changed: 120 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -74,12 +74,128 @@ void MoveGroupMoveAction::initialize()
7474
void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr<MGActionGoal>& goal)
7575
{
7676
RCLCPP_INFO(LOGGER, "executing..");
77+
78+
// Print a simple debug message for the goal
79+
RCLCPP_INFO(LOGGER, "Goal received.");
80+
81+
// Debugging: Print each field of the request
82+
const auto& request = goal->get_goal()->request;
83+
84+
RCLCPP_INFO_STREAM(LOGGER, "Request pipeline_id: " << request.pipeline_id);
85+
RCLCPP_INFO_STREAM(LOGGER, "Request group_name: " << request.group_name);
86+
RCLCPP_INFO_STREAM(LOGGER, "Request num_planning_attempts: " << request.num_planning_attempts);
87+
RCLCPP_INFO_STREAM(LOGGER, "Request allowed_planning_time: " << request.allowed_planning_time);
88+
RCLCPP_INFO_STREAM(LOGGER, "Request max_velocity_scaling_factor: " << request.max_velocity_scaling_factor);
89+
RCLCPP_INFO_STREAM(LOGGER, "Request max_acceleration_scaling_factor: " << request.max_acceleration_scaling_factor);
90+
91+
// Print start_state details
92+
const auto& start_state = request.start_state;
93+
RCLCPP_INFO_STREAM(LOGGER, "Request start_state is set: " << !moveit::core::isEmpty(start_state));
94+
95+
// Log joint names
96+
std::ostringstream joint_names_stream;
97+
for (const auto& name : start_state.joint_state.name)
98+
joint_names_stream << name << " ";
99+
RCLCPP_INFO_STREAM(LOGGER, "Start state joint names: " << joint_names_stream.str());
100+
101+
// Log joint positions
102+
std::ostringstream joint_positions_stream;
103+
for (const auto& position : start_state.joint_state.position)
104+
joint_positions_stream << position << " ";
105+
RCLCPP_INFO_STREAM(LOGGER, "Start state joint positions: " << joint_positions_stream.str());
106+
107+
// Print goal_constraints details
108+
RCLCPP_INFO_STREAM(LOGGER, "Request goal_constraints size: " << request.goal_constraints.size());
109+
for (size_t i = 0; i < request.goal_constraints.size(); ++i)
110+
{
111+
const auto& constraint = request.goal_constraints[i];
112+
RCLCPP_INFO_STREAM(LOGGER, "Goal constraint [" << i << "] joint_constraints size: " << constraint.joint_constraints.size());
113+
for (size_t j = 0; j < constraint.joint_constraints.size(); ++j)
114+
{
115+
const auto& joint_constraint = constraint.joint_constraints[j];
116+
RCLCPP_INFO_STREAM(LOGGER, " Joint constraint [" << j << "] name: " << joint_constraint.joint_name);
117+
RCLCPP_INFO_STREAM(LOGGER, " Joint constraint [" << j << "] position: " << joint_constraint.position);
118+
}
119+
RCLCPP_INFO_STREAM(LOGGER, "Goal constraint [" << i << "] position_constraints size: " << constraint.position_constraints.size());
120+
// Add similar detailed logging for position_constraints, orientation_constraints, etc., if needed.
121+
}
122+
123+
// Print path_constraints details
124+
const auto& path_constraints = request.path_constraints;
125+
RCLCPP_INFO_STREAM(LOGGER, "Request path_constraints joint_constraints size: " << path_constraints.joint_constraints.size());
126+
for (size_t i = 0; i < path_constraints.joint_constraints.size(); ++i)
127+
{
128+
const auto& joint_constraint = path_constraints.joint_constraints[i];
129+
RCLCPP_INFO_STREAM(LOGGER, " Joint constraint [" << i << "] name: " << joint_constraint.joint_name);
130+
RCLCPP_INFO_STREAM(LOGGER, " Joint constraint [" << i << "] position: " << joint_constraint.position);
131+
}
132+
RCLCPP_INFO_STREAM(LOGGER, "Request path_constraints position_constraints size: " << path_constraints.position_constraints.size());
133+
for (size_t i = 0; i < path_constraints.position_constraints.size(); ++i)
134+
{
135+
const auto& position_constraint = path_constraints.position_constraints[i];
136+
RCLCPP_INFO_STREAM(LOGGER, " Position constraint [" << i << "] frame_id: " << position_constraint.header.frame_id);
137+
RCLCPP_INFO_STREAM(LOGGER, " Position constraint [" << i << "] target point offset: "
138+
<< position_constraint.target_point_offset.x << ", "
139+
<< position_constraint.target_point_offset.y << ", "
140+
<< position_constraint.target_point_offset.z);
141+
}
142+
RCLCPP_INFO_STREAM(LOGGER, "Request path_constraints orientation_constraints size: " << path_constraints.orientation_constraints.size());
143+
for (size_t i = 0; i < path_constraints.orientation_constraints.size(); ++i)
144+
{
145+
const auto& orientation_constraint = path_constraints.orientation_constraints[i];
146+
RCLCPP_INFO_STREAM(LOGGER, " Orientation constraint [" << i << "] frame_id: " << orientation_constraint.header.frame_id);
147+
RCLCPP_INFO_STREAM(LOGGER, " Orientation constraint [" << i << "] orientation: "
148+
<< orientation_constraint.orientation.x << ", "
149+
<< orientation_constraint.orientation.y << ", "
150+
<< orientation_constraint.orientation.z << ", "
151+
<< orientation_constraint.orientation.w);
152+
}
153+
154+
// Print trajectory_constraints details
155+
const auto& trajectory_constraints = request.trajectory_constraints;
156+
RCLCPP_INFO_STREAM(LOGGER, "Request trajectory_constraints constraints size: " << trajectory_constraints.constraints.size());
157+
for (size_t i = 0; i < trajectory_constraints.constraints.size(); ++i)
158+
{
159+
const auto& constraint = trajectory_constraints.constraints[i];
160+
RCLCPP_INFO_STREAM(LOGGER, " Trajectory constraint [" << i << "] joint_constraints size: " << constraint.joint_constraints.size());
161+
for (size_t j = 0; j < constraint.joint_constraints.size(); ++j)
162+
{
163+
const auto& joint_constraint = constraint.joint_constraints[j];
164+
RCLCPP_INFO_STREAM(LOGGER, " Joint constraint [" << j << "] name: " << joint_constraint.joint_name);
165+
RCLCPP_INFO_STREAM(LOGGER, " Joint constraint [" << j << "] position: " << joint_constraint.position);
166+
}
167+
RCLCPP_INFO_STREAM(LOGGER, " Trajectory constraint [" << i << "] position_constraints size: " << constraint.position_constraints.size());
168+
for (size_t j = 0; j < constraint.position_constraints.size(); ++j)
169+
{
170+
const auto& position_constraint = constraint.position_constraints[j];
171+
RCLCPP_INFO_STREAM(LOGGER, " Position constraint [" << j << "] frame_id: " << position_constraint.header.frame_id);
172+
RCLCPP_INFO_STREAM(LOGGER, " Position constraint [" << j << "] target point offset: "
173+
<< position_constraint.target_point_offset.x << ", "
174+
<< position_constraint.target_point_offset.y << ", "
175+
<< position_constraint.target_point_offset.z);
176+
}
177+
RCLCPP_INFO_STREAM(LOGGER, " Trajectory constraint [" << i << "] orientation_constraints size: " << constraint.orientation_constraints.size());
178+
for (size_t j = 0; j < constraint.orientation_constraints.size(); ++j)
179+
{
180+
const auto& orientation_constraint = constraint.orientation_constraints[j];
181+
RCLCPP_INFO_STREAM(LOGGER, " Orientation constraint [" << j << "] frame_id: " << orientation_constraint.header.frame_id);
182+
RCLCPP_INFO_STREAM(LOGGER, " Orientation constraint [" << j << "] orientation: "
183+
<< orientation_constraint.orientation.x << ", "
184+
<< orientation_constraint.orientation.y << ", "
185+
<< orientation_constraint.orientation.z << ", "
186+
<< orientation_constraint.orientation.w);
187+
}
188+
}
189+
190+
RCLCPP_INFO_STREAM(LOGGER, "Request cartesian_speed_end_effector_link: " << request.cartesian_speed_end_effector_link);
191+
RCLCPP_INFO_STREAM(LOGGER, "Request max_cartesian_speed: " << request.max_cartesian_speed);
192+
77193
setMoveState(PLANNING, goal);
78194
// before we start planning, ensure that we have the latest robot state received...
79195
auto node = context_->moveit_cpp_->getNode();
80196
context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now());
81197
context_->planning_scene_monitor_->updateFrameTransforms();
82-
198+
83199
auto action_res = std::make_shared<MGAction::Result>();
84200
if (goal->get_goal()->planning_options.plan_only || !context_->allow_trajectory_execution_)
85201
{
@@ -91,7 +207,7 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr<MGActionGoal
91207
}
92208
else
93209
executeMoveCallbackPlanAndExecute(goal, action_res);
94-
210+
95211
bool planned_trajectory_empty = trajectory_processing::isTrajectoryEmpty(action_res->planned_trajectory);
96212
// @todo: Response messages
97213
RCLCPP_INFO_STREAM(LOGGER, getActionResultString(action_res->error_code, planned_trajectory_empty,
@@ -102,10 +218,11 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr<MGActionGoal
102218
goal->canceled(action_res);
103219
else
104220
goal->abort(action_res);
105-
221+
106222
setMoveState(IDLE, goal);
107223
preempt_requested_ = false;
108224
}
225+
109226

110227
void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_ptr<MGActionGoal>& goal,
111228
std::shared_ptr<MGAction::Result>& action_res)

0 commit comments

Comments
 (0)