Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/rolling' into yadu/omnicore
Browse files Browse the repository at this point in the history
  • Loading branch information
Yadunund committed Jan 8, 2024
2 parents a8bf3a8 + fd9a44c commit b8dda62
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 87 deletions.
155 changes: 69 additions & 86 deletions abb_bringup/launch/abb_moveit.launch.py
Original file line number Diff line number Diff line change
@@ -1,17 +1,15 @@
import os

import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os
import yaml
from moveit_configs_utils import MoveItConfigsBuilder


def load_yaml(package_name, file_path):
Expand All @@ -32,94 +30,84 @@ def launch_setup(context, *args, **kwargs):
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")

# Planning context
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(support_package), "urdf", robot_xacro_file]
),
]
)
robot_description = {"robot_description": robot_description_content}

robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", moveit_config_file]
),
]
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content.perform(
context
# MoveIt configuration
moveit_config = (
MoveItConfigsBuilder(
"abb_bringup", package_name=f"{moveit_config_package.perform(context)}"
)
}

kinematics_yaml = load_yaml(
"abb_irb1200_5_90_moveit_config", "config/kinematics.yaml"
)

joint_limits_yaml = {
"robot_description_planning": load_yaml(
moveit_config_package.perform(context), "config/joint_limits.yaml"
.robot_description(
file_path=os.path.join(
get_package_share_directory(f"{support_package.perform(context)}"),
"urdf",
f"{robot_xacro_file.perform(context)}",
)
)
}

# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(
"abb_irb1200_5_90_moveit_config", "config/ompl_planning.yaml"
.robot_description_semantic(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
f"{moveit_config_file.perform(context)}",
)
)
.planning_pipelines()
.robot_description_kinematics(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
"kinematics.yaml",
)
)
.trajectory_execution(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
"moveit_controllers.yaml",
)
)
.planning_scene_monitor(
publish_planning_scene=True,
publish_geometry_updates=True,
publish_state_updates=True,
publish_transforms_updates=True,
publish_robot_description=True,
publish_robot_description_semantic=True,
)
.joint_limits(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
"joint_limits.yaml",
)
)
.to_moveit_configs()
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)

# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"abb_irb1200_5_90_moveit_config", "config/moveit_controllers.yaml"
)
# MoveIt controllers
moveit_controllers = {
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
"moveit_simple_controller_manager": load_yaml(
f"{moveit_config_package.perform(context)}",
"config/moveit_controllers.yaml",
),
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}

trajectory_execution = {
# MoveIt does not handle controller switching automatically
"moveit_manage_controllers": False,
"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_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}

# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_config.trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
joint_limits_yaml,
moveit_config.to_dict(),
],
)

Expand All @@ -135,11 +123,7 @@ def launch_setup(context, *args, **kwargs):
output="log",
arguments=["-d", rviz_config],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
joint_limits_yaml,
moveit_config.to_dict(),
],
)

Expand All @@ -158,15 +142,14 @@ def launch_setup(context, *args, **kwargs):
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
parameters=[moveit_config.robot_description],
)

nodes_to_start = [move_group_node, rviz_node, static_tf_node, robot_state_pub_node]
return nodes_to_start


def generate_launch_description():

declared_arguments = []

# TODO(andyz): add other options
Expand Down
1 change: 1 addition & 0 deletions abb_bringup/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
<exec_depend>rviz2</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>moveit_configs_utils</exec_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
planning_plugin: ompl_interface/OMPLPlanner
request_adapters: >-
default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/AddTimeOptimalParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
cartesian_limits:
max_trans_vel: 1.0
max_trans_acc: 2.25
max_trans_dec: -5.0
max_rot_vel: 1.57

0 comments on commit b8dda62

Please sign in to comment.