Skip to content

Clean up move_group and moveit_cpp tutorials #93

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Jul 2, 2021
Merged

Clean up move_group and moveit_cpp tutorials #93

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bf824ec
cleaned up move_group and moveit_cpp tutorials
MarqRazz Jun 29, 2021
b1f512a
run pre-commit
MarqRazz Jun 29, 2021
79bfcac
revert back to joint_state_controller
MarqRazz Jul 1, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion doc/move_group_interface/launch/move_group.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,11 @@ def generate_launch_description():

# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
for controller in [
"panda_arm_controller",
"panda_hand_controller",
"joint_state_controller",
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
Expand Down
24 changes: 13 additions & 11 deletions doc/move_group_interface/src/move_group_interface_tutorial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ int main(int argc, char** argv)
// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.0;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);

// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
Expand Down Expand Up @@ -165,7 +165,7 @@ int main(int argc, char** argv)
// We can also visualize the plan as a line with markers in RViz.
RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */
visual_tools.trigger();
prompt("Press 'Enter' to continue the demo");
Expand Down Expand Up @@ -215,7 +215,7 @@ int main(int argc, char** argv)

// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */
visual_tools.trigger();
prompt("Press 'Enter' to continue the demo");
Expand Down Expand Up @@ -285,7 +285,7 @@ int main(int argc, char** argv)
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */
visual_tools.trigger();
prompt("Press 'Enter' to continue the demo");
Expand Down Expand Up @@ -329,7 +329,7 @@ int main(int argc, char** argv)

// Visualize the plan in RViz
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
Expand Down Expand Up @@ -362,7 +362,7 @@ int main(int argc, char** argv)
RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Clear Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(another_pose, "goal");
/* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */
visual_tools.trigger();
Expand Down Expand Up @@ -409,15 +409,15 @@ int main(int argc, char** argv)
planning_scene_interface.addCollisionObjects(collision_objects);

// Show text in RViz of status and wait for MoveGroup to receive and process the collision object message
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
prompt("Press 'Enter' to continue once the collision object appears in RViz");
/* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); */

// Now when we plan a trajectory it will avoid the obstacle
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED");
visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); */
visual_tools.trigger();
prompt("Press 'Enter' to continue once the plan is complete");
Expand Down Expand Up @@ -464,7 +464,7 @@ int main(int argc, char** argv)
touch_links.push_back("panda_leftfinger");
move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);

visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to receive and process the attached collision object message */
Expand Down Expand Up @@ -494,7 +494,7 @@ int main(int argc, char** argv)

// Show text in RViz of status
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to receive and process the attached collision object message */
Expand All @@ -509,14 +509,16 @@ int main(int argc, char** argv)
planning_scene_interface.removeCollisionObjects(object_ids);

// Show text in RViz of status
visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to receive and process the attached collision object message */
prompt("Press 'Enter' once the collision object disappears");
/* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); */

// END_TUTORIAL
visual_tools.deleteAllMarkers();
visual_tools.trigger();

rclcpp::shutdown();
return 0;
Expand Down
Binary file added doc/moveit_cpp/images/moveitcpp_plan5.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
7 changes: 5 additions & 2 deletions doc/moveit_cpp/launch/moveit_cpp_tutorial.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,6 @@ def generate_launch_description():
kinematics_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/kinematics.yaml"
)
robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}

moveit_simple_controllers_yaml = load_yaml(
"moveit_resources_panda_moveit_config", "config/panda_controllers.yaml"
Expand Down Expand Up @@ -144,7 +143,11 @@ def generate_launch_description():

# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
for controller in [
"panda_arm_controller",
"panda_hand_controller",
"joint_state_controller",
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
Expand Down
2 changes: 1 addition & 1 deletion doc/moveit_cpp/launch/moveit_cpp_tutorial.rviz
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ Visualization Manager:
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /moveit_cpp/publish_planning_scene
Planning Scene Topic: /moveit_cpp/monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Expand Down
85 changes: 69 additions & 16 deletions doc/moveit_cpp/src/moveit_cpp_tutorial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,11 +56,10 @@ int main(int argc, char** argv)

RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");

auto moveit_cpp_ptr = std::make_shared<moveit::planning_interface::MoveItCpp>(node);
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(node);
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();

auto planning_components =
std::make_shared<moveit::planning_interface::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
auto robot_start_state = planning_components->getStartState();
auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
Expand All @@ -78,7 +77,7 @@ int main(int argc, char** argv)

Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveItCpp Demo", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

// Start the demo
Expand Down Expand Up @@ -115,10 +114,9 @@ int main(int argc, char** argv)
{
// Visualize the start pose in Rviz
visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "start_pose");
visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
// Visualize the goal pose in Rviz
visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishText(text_pose, "setStartStateToCurrentState", rvt::WHITE, rvt::XLARGE);
// Visualize the trajectory in Rviz
/* visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr); */
visual_tools.trigger();
Expand All @@ -134,9 +132,10 @@ int main(int argc, char** argv)
// :align: center
//
// Start the next plan
visual_tools.deleteAllMarkers();
prompt("Press 'Enter' to continue the demo");
/* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
visual_tools.deleteAllMarkers();
visual_tools.trigger();

// Plan #2
// ^^^^^^^
Expand All @@ -161,10 +160,9 @@ int main(int argc, char** argv)
moveit::core::RobotState robot_state(robot_model_ptr);
moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state);

visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr); */
visual_tools.trigger();

Expand All @@ -179,9 +177,10 @@ int main(int argc, char** argv)
// :align: center
//
// Start the next plan
visual_tools.deleteAllMarkers();
prompt("Press 'Enter' to continue the demo");
/* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
visual_tools.deleteAllMarkers();
visual_tools.trigger();

// Plan #3
// ^^^^^^^
Expand All @@ -206,10 +205,9 @@ int main(int argc, char** argv)
moveit::core::RobotState robot_state(robot_model_ptr);
moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state);

visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(target_pose2, "target_pose");
visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr); */
visual_tools.trigger();

Expand All @@ -224,9 +222,10 @@ int main(int argc, char** argv)
// :align: center
//
// Start the next plan
visual_tools.deleteAllMarkers();
prompt("Press 'Enter' to continue the demo");
/* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
visual_tools.deleteAllMarkers();
visual_tools.trigger();

// Plan #4
// ^^^^^^^
Expand All @@ -249,10 +248,9 @@ int main(int argc, char** argv)
moveit::core::RobotState robot_state(robot_model_ptr);
moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state);

visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");
visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose");
visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr); */
visual_tools.trigger();

Expand All @@ -266,10 +264,65 @@ int main(int argc, char** argv)
// :width: 250pt
// :align: center
//
// END_TUTORIAL
// Start the next plan
prompt("Press 'Enter' to continue the demo");
/* visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); */
visual_tools.deleteAllMarkers();
visual_tools.trigger();

// Plan #5
// ^^^^^^^
//
// We can also generate motion plans around objects in the collision scene.
//
// First we create the collision object
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "panda_link0";
collision_object.id = "box";

shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = { 0.1, 0.4, 0.1 };

geometry_msgs::msg::Pose box_pose;
box_pose.position.x = 0.4;
box_pose.position.y = 0.0;
box_pose.position.z = 1.0;

collision_object.primitives.push_back(box);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

// Add object to planning scene
{ // Lock PlanningScene
planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor());
scene->processCollisionObjectMsg(collision_object);
} // Unlock PlanningScene
planning_components->setStartStateToCurrentState();
planning_components->setGoal("extended");

auto plan_solution5 = planning_components->plan();
if (plan_solution5)
{
visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE);
/* visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr); */
visual_tools.trigger();

/* Uncomment if you want to execute the plan */
/* planning_components->execute(); // Execute the plan */
}

// Plan #5 visualization:
//
// .. image:: images/moveitcpp_plan5.png
// :width: 250pt
// :align: center
//
// END_TUTORIAL
prompt("Press 'Enter' to exit the demo");
/* visual_tools.prompt("Press 'next' to end the demo"); */
visual_tools.deleteAllMarkers();
visual_tools.trigger();

RCLCPP_INFO(LOGGER, "Shutting down.");
rclcpp::shutdown();
Expand Down
6 changes: 5 additions & 1 deletion doc/quickstart_in_rviz/launch/demo.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,11 @@ def generate_launch_description():

# Load controllers
load_controllers = []
for controller in ["panda_arm_controller", "joint_state_controller"]:
for controller in [
"panda_arm_controller",
"panda_hand_controller",
"joint_state_controller",
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
Expand Down