@@ -143,7 +143,7 @@ int main(int argc, char** argv)
143
143
// to actually move the robot.
144
144
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
145
145
146
- bool success = (move_group.plan (my_plan) == moveit::planning_interface ::MoveItErrorCode::SUCCESS);
146
+ bool success = (move_group.plan (my_plan) == moveit::core ::MoveItErrorCode::SUCCESS);
147
147
148
148
RCLCPP_INFO (LOGGER, " Visualizing plan 1 (pose goal) %s" , success ? " " : " FAILED" );
149
149
@@ -196,7 +196,7 @@ int main(int argc, char** argv)
196
196
move_group.setMaxVelocityScalingFactor (0.05 );
197
197
move_group.setMaxAccelerationScalingFactor (0.05 );
198
198
199
- success = (move_group.plan (my_plan) == moveit::planning_interface ::MoveItErrorCode::SUCCESS);
199
+ success = (move_group.plan (my_plan) == moveit::core ::MoveItErrorCode::SUCCESS);
200
200
RCLCPP_INFO (LOGGER, " Visualizing plan 2 (joint space goal) %s" , success ? " " : " FAILED" );
201
201
202
202
// Visualize the plan in RViz:
@@ -263,7 +263,7 @@ int main(int argc, char** argv)
263
263
// Let's increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
264
264
move_group.setPlanningTime (10.0 );
265
265
266
- success = (move_group.plan (my_plan) == moveit::planning_interface ::MoveItErrorCode::SUCCESS);
266
+ success = (move_group.plan (my_plan) == moveit::core ::MoveItErrorCode::SUCCESS);
267
267
RCLCPP_INFO (LOGGER, " Visualizing plan 3 (constraints) %s" , success ? " " : " FAILED" );
268
268
269
269
// Visualize the plan in RViz:
@@ -341,7 +341,7 @@ int main(int argc, char** argv)
341
341
another_pose.position .z = 0.59 ;
342
342
move_group.setPoseTarget (another_pose);
343
343
344
- success = (move_group.plan (my_plan) == moveit::planning_interface ::MoveItErrorCode::SUCCESS);
344
+ success = (move_group.plan (my_plan) == moveit::core ::MoveItErrorCode::SUCCESS);
345
345
RCLCPP_INFO (LOGGER, " Visualizing plan 5 (with no obstacles) %s" , success ? " " : " FAILED" );
346
346
347
347
visual_tools.deleteAllMarkers ();
@@ -396,7 +396,7 @@ int main(int argc, char** argv)
396
396
visual_tools.prompt (" Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz" );
397
397
398
398
// Now, when we plan a trajectory it will avoid the obstacle.
399
- success = (move_group.plan (my_plan) == moveit::planning_interface ::MoveItErrorCode::SUCCESS);
399
+ success = (move_group.plan (my_plan) == moveit::core ::MoveItErrorCode::SUCCESS);
400
400
RCLCPP_INFO (LOGGER, " Visualizing plan 6 (pose goal move around cuboid) %s" , success ? " " : " FAILED" );
401
401
visual_tools.publishText (text_pose, " Obstacle_Goal" , rvt::WHITE, rvt::XLARGE);
402
402
visual_tools.publishTrajectoryLine (my_plan.trajectory_ , joint_model_group);
@@ -452,7 +452,7 @@ int main(int argc, char** argv)
452
452
453
453
// Replan, but now with the object in hand.
454
454
move_group.setStartStateToCurrentState ();
455
- success = (move_group.plan (my_plan) == moveit::planning_interface ::MoveItErrorCode::SUCCESS);
455
+ success = (move_group.plan (my_plan) == moveit::core ::MoveItErrorCode::SUCCESS);
456
456
RCLCPP_INFO (LOGGER, " Visualizing plan 7 (move around cuboid with cylinder) %s" , success ? " " : " FAILED" );
457
457
visual_tools.publishTrajectoryLine (my_plan.trajectory_ , joint_model_group);
458
458
visual_tools.trigger ();
0 commit comments