Skip to content

Commit

Permalink
Merge pull request #365 from jdlangs/feature/exercise-4.0-instructions
Browse files Browse the repository at this point in the history 
Update exercise 4.0 instructions
  • Loading branch information
@jdlangs
jdlangs committed Oct 6, 2021
2 parents 740bda9 + 820003a commit 624ad29
Show file tree
Hide file tree
Showing 2 changed files with 174 additions and 60 deletions.
6 changes: 2 additions & 4 deletions exercises/4.0/ros2/src/myworkcell_core/src/myworkcell_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,14 +73,12 @@ class ScanNPlan : public rclcpp::Node
}
moveit::core::RobotStatePtr start_robot_state = moveit_cpp_->getCurrentState(2.0);

// setting start robot state
planning_component_->setStartState(*start_robot_state);

// Set motion goal of end effector link
std::string ee_link = moveit_cpp_->getRobotModel()->getJointModelGroup(
planning_component_->getPlanningGroupName())->getLinkModelNames().back();

RCLCPP_INFO(this->get_logger(), "Setting move target pose for tip link \"%s\"", ee_link.c_str());

planning_component_->setStartState(*start_robot_state);
planning_component_->setGoal(move_target, ee_link);

// Now we can plan!
Expand Down
228 changes: 172 additions & 56 deletions gh_pages/_source/session4/ros2/0-Motion-Planning-CPP.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,63 +15,120 @@ Now that we’ve got a working MoveIt! configuration for your workcell and we’
## Scan-N-Plan Application: Problem Statement
In this exercise, your goal is to modify the `myworkcell_core` node to:

1. Use the MoveItCpp API to enable planning and execution of the robot from within your C++ program.
1. Move the robot’s tool frame to the center of the part location as reported by the service call to your vision node.

## Scan-N-Plan Application: Guidance

### Using MoveGroupInterface
### MoveItCpp

1. Edit your `myworkcell_node.cpp` file. In the `ScanNPlan` class's `start` method, use the response from the `LocalizePart` service to initialize a new `move_target` variable:
For this exercise we will use MoveIt's *MoveItCpp* API, which lets us directly call into the MoveIt libraries from our C++ application. This API is new in MoveIt2. The previous method of the *MoveGroupInterface* API, which calls a separate move group node through ROS services and actions is still available, though it's not the recommended interface.

``` c++
geometry_msgs::msg::PoseStamped move_target;
move_target.header.frame_id = base_frame;
move_target.pose = response->pose;
1. Add dependencies on the MoveIt packages `moveit_msgs` and `moveit_ros_planning_interface` to `myworkcell_core/CMakeLists.txt`:

```
find_package(moveit_msgs REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
...
ament_target_dependencies(myworkcell_node
rclcpp
moveit_msgs
moveit_ros_planning_interface
)
```

* make sure to place this code _after_ the call to the vision_node's service.
and to `package.xml`:

1. Create a `MoveGroupInterface` object which will be used for performing the motion planning
```
<depend>moveit_msgs</depend>
<depend>moveit_ros_planning_interface</depend>
```

1. Add these lines in the `start` function:
1. Open `myworkcell_core/src/myworkcell_node.cpp`. We'll first add the needed MoveItCpp objects as new class members of the node. Add the following lines in the `private` section of the `ScanNPlan` node:

```
moveit::planning_interface::MoveGroupInterface move_group(
this->shared_from_this(),
"manipulator");
```c++
moveit_cpp::MoveItCppPtr moveit_cpp_;
moveit_cpp::PlanningComponentPtr planning_component_;
moveit_cpp::PlanningComponent::PlanRequestParameters plan_parameters_;
```

move_group.setPoseTarget(move_target);
move_group.move();
```
1. Add the required includes for these objects at the top of the file:

1. Add required dependencies in your `CMakeLists.txt`:
```c++
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
```

1. Create a new `setup()` function inside `ScanNPlan` after the constructor and above `start`. We'll use this function to initialize the MoveIt objects after the node has started.

```
find_package(moveit_msgs REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
```c++
// MoveIt setup
void setup()
{
// Instantiate moveit_cpp
moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(this->shared_from_this());

...
// Planning component associated with a single motion group
planning_component_ = std::make_shared<moveit_cpp::PlanningComponent>("manipulator", moveit_cpp_);

ament_target_dependencies(myworkcell_node
rclcpp
moveit_msgs
moveit_ros_planning_interface
)
target_link_libraries(myworkcell_node Boost::system)
```
// Parameters set on this node
plan_parameters_.load(this->shared_from_this());
}
```

### Planning

1. In the `ScanNPlan` class's `start` method, use the response from the `LocalizePart` service to initialize a new `move_target` variable:

``` c++
geometry_msgs::msg::PoseStamped move_target;
move_target.header.frame_id = base_frame;
move_target.pose = response->pose;
```

* Make sure to place this code _after_ the call to the vision_node's service.
* You may need to provide another include for this datatype: `#include <geometry_msgs/msg/pose_stamped.hpp>`.

1. We'll use the `PlanningComponent` object to plan to this target, but it needs additional information about where the robot will start from and which part of the robot should move to the target. Add the following lines to get the current robot state and the name of end effector link:

```c++
// getting current state of robot from environment
if (!moveit_cpp_->getPlanningSceneMonitor()->requestPlanningSceneState())
{
RCLCPP_ERROR(this->get_logger(), "Failed to get planning scene");
return;
}
moveit::core::RobotStatePtr start_robot_state = moveit_cpp_->getCurrentState(2.0);

// Set motion goal of end effector link
std::string ee_link = moveit_cpp_->getRobotModel()->getJointModelGroup(
planning_component_->getPlanningGroupName())->getLinkModelNames().back();
```

and in `package.xml`:
1. Now we can add the main functionality, the calls to plan and execute:

```
<depend>moveit_msgs</depend>
<depend>moveit_ros_planning_interface</depend>
```
```c++
planning_component_->setStartState(*start_robot_state);
planning_component_->setGoal(move_target, ee_link);

1. Add includes at the top of file for the needed MoveIt components:
// Now we can plan!
moveit_cpp::PlanningComponent::PlanSolution plan_solution = planning_component_->plan(plan_parameters_);
if (!plan_solution)
{
RCLCPP_ERROR(this->get_logger(),"Failed to plan");
return;
}

```
#include <moveit/move_group_interface/move_group_interface.h>
```
// If planning succeeded, execute the returned trajectory
bool success = moveit_cpp_->execute("manipulator", plan_solution.trajectory, true);
if (!success)
{
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to execute trajectory");
return;
}
```

1. Currently, MoveIt2 uses many parameters that are not declared ahead of time. To enable this, we have to construct our ROS2 node with an option to automatically declare parameters when they are set in a launch file. Modify the `ScanNPlan` constructor to start with the following:

Expand All @@ -88,7 +145,7 @@ In this exercise, your goal is to modify the `myworkcell_core` node to:

### Execution

1. The `move` command is all that's needed to get your manipulator to move (with the right parameters) but some support when running MoveIt to have it function properly. Inside the `main` function, insert the following lines before you call `app->start`:
1. The `plan` and `execute` functions are all that's needed to get your manipulator to move (with the right parameters) but some support when running MoveIt is needed to have it function properly. Inside the `main` function, insert the following lines before you call `app->start`:

```
// Start spinning in a background thread so MoveIt internals can execute
Expand All @@ -98,6 +155,9 @@ In this exercise, your goal is to modify the `myworkcell_core` node to:
rclcpp::spin(app);
}
};
// Perform MoveIt initialization
app->setup();
```

This lets ROS process callbacks in a separate worker thread while the main thread remains available for us to define our application logic. This implies we can no longer call any spin functions in the main thread now. Replace the call to `spin_until_future_complete` in the `start` function with a function to simply wait for the future to be ready:
Expand All @@ -114,8 +174,9 @@ In this exercise, your goal is to modify the `myworkcell_core` node to:
```
import os
import yaml
import launch
import launch_ros
import xacro
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python import get_package_share_directory
def get_package_file(package, file_path):
Expand Down Expand Up @@ -148,31 +209,67 @@ In this exercise, your goal is to modify the `myworkcell_core` node to:
os.system(f'xacro {xacro_file} -o {urdf_file}')
return urdf_file
def generate_launch_description():
xacro_file = get_package_file('myworkcell_support', 'urdf/workcell.urdf.xacro')
urdf_file = run_xacro(xacro_file)
srdf_file = get_package_file('myworkcell_moveit_config', 'config/myworkcell.srdf')
kinematics_file = get_package_file('myworkcell_moveit_config', 'config/kinematics.yaml')
ompl_config_file = get_package_file('myworkcell_moveit_config', 'config/ompl_planning.yaml')
joint_limits_file = get_package_file('myworkcell_moveit_config','config/joint_limits.yaml')
moveit_controllers_file = get_package_file('myworkcell_moveit_config', 'config/controllers.yaml')
robot_description = load_file(urdf_file)
robot_description_semantic = load_file(srdf_file)
kinematics_config = load_yaml(kinematics_file)
return launch.LaunchDescription([
launch_ros.actions.Node(
name='fake_ar_publisher_node',
package='fake_ar_publisher',
executable='fake_ar_publisher_node',
output='screen',
),
launch_ros.actions.Node(
name='vision_node',
package='myworkcell_core',
executable='vision_node',
output='screen',
),
launch_ros.actions.Node(
ompl_config = load_yaml(ompl_config_file)
joint_limits_config = load_yaml(joint_limits_file)
# Setting up MoveitCpp configuration parameters
moveit_controllers = {
'moveit_simple_controller_manager' : load_yaml(moveit_controllers_file),
'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
}
trajectory_execution = {
'moveit_manage_controllers': True,
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
'trajectory_execution.allowed_goal_duration_margin': 0.5,
'trajectory_execution.allowed_start_tolerance': 0.01
}
planning_scene_monitor_config = {
'publish_planning_scene': True,
'publish_geometry_updates': True,
'publish_state_updates': True,
'publish_transforms_updates': True
}
moveit_cpp_config = yaml.load("""
planning_scene_monitor_options:
name: "planning_scene_monitor"
robot_description: "robot_description"
joint_state_topic: "/joint_states"
attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
wait_for_initial_state_timeout: 10.0
planning_pipelines:
#namespace: "moveit_cpp" # optional, default is ~
pipeline_names: ["ompl"]
plan_request_params:
planning_time: 10.0
planning_attempts: 3
planning_pipeline: ompl
max_velocity_scaling_factor: 0.5
max_acceleration_scaling_factor: 0.5
# octomap parameters (when used)
octomap_frame: world
octomap_resolution: 0.01
max_range: 5.0""")
return LaunchDescription([
Node(
name='myworkcell_node',
package='myworkcell_core',
executable='myworkcell_node',
Expand All @@ -183,9 +280,28 @@ In this exercise, your goal is to modify the `myworkcell_core` node to:
'robot_description': robot_description,
'robot_description_semantic': robot_description_semantic,
'robot_description_kinematics': kinematics_config,
'robot_description_planning' : joint_limits_config,
'planning_pipelines': ['ompl'],
'ompl': ompl_config
},
moveit_cpp_config,
moveit_controllers,
trajectory_execution,
planning_scene_monitor_config,
],
),
Node(
name='fake_ar_publisher_node',
package='fake_ar_publisher',
executable='fake_ar_publisher_node',
output='screen',
),
Node(
name='vision_node',
package='myworkcell_core',
executable='vision_node',
output='screen',
),
])
```

Expand Down

0 comments on commit 624ad29

Please sign in to comment.