Skip to content

Commit

Permalink
Run pre-commit formatting
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
methylDragon committed Aug 9, 2024
1 parent 935b7a9 commit 474b242
Show file tree
Hide file tree
Showing 5 changed files with 62 additions and 63 deletions.
2 changes: 1 addition & 1 deletion doc/how_to_guides/trajectory_cache/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -3,4 +3,4 @@ ament_target_dependencies(trajectory_cache_demo ${THIS_PACKAGE_INCLUDE_DEPENDS}

install(TARGETS trajectory_cache_demo DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
Original file line number Diff line number Diff line change
Expand Up @@ -53,4 +53,4 @@ joint_limits:
max_velocity: 25.0
has_acceleration_limits: true
max_acceleration: 5.0
has_jerk_limits: false
has_jerk_limits: false
Original file line number Diff line number Diff line change
Expand Up @@ -20,81 +20,77 @@

configurable_parameters = {
# Cache DB
'cache_db_plugin': {
'default': 'warehouse_ros_sqlite::DatabaseConnection',
'description': 'Plugin to use for the trajectory cache database.'
"cache_db_plugin": {
"default": "warehouse_ros_sqlite::DatabaseConnection",
"description": "Plugin to use for the trajectory cache database.",
},
'cache_db_host': {
'default': '":memory:"',
'description': 'Host for the trajectory cache database. Use ":memory:" for an in-memory database.'
"cache_db_host": {
"default": '":memory:"',
"description": 'Host for the trajectory cache database. Use ":memory:" for an in-memory database.',
},
'cache_db_port': {
'default': '0',
'description': 'Port for the trajectory cache database.'
"cache_db_port": {
"default": "0",
"description": "Port for the trajectory cache database.",
},

# Reconfigurable (these can be set at runtime and will update)
'start_tolerance': { # Trajectory cache param
'default': '0.025',
'description': 'Reconfigurable cache param. Tolerance for the start state of the trajectory.'
"start_tolerance": { # Trajectory cache param
"default": "0.025",
"description": "Reconfigurable cache param. Tolerance for the start state of the trajectory.",
},
'goal_tolerance': { # Trajectory cache param
'default': '0.001',
'description': 'Reconfigurable cache param. Tolerance for the goal state of the trajectory.'
"goal_tolerance": { # Trajectory cache param
"default": "0.001",
"description": "Reconfigurable cache param. Tolerance for the goal state of the trajectory.",
},
'prune_worse_trajectories': { # Trajectory cache param
'default': 'false',
'description': 'Reconfigurable cache param. Whether to delete trajectories that are worse than the current trajectory.'
"prune_worse_trajectories": { # Trajectory cache param
"default": "false",
"description": "Reconfigurable cache param. Whether to delete trajectories that are worse than the current trajectory.",
},
'planner': {
'default': 'RRTstar',
'description': 'Reconfigurable. Planner to use for trajectory planning.'
"planner": {
"default": "RRTstar",
"description": "Reconfigurable. Planner to use for trajectory planning.",
},

# Tutorial
'num_target_poses': {
'default': '4',
'description': 'Number of target poses to generate.'
"num_target_poses": {
"default": "4",
"description": "Number of target poses to generate.",
},
'num_cartesian_target_paths_per_target_pose': {
'default': '2', 'description': 'Number of Cartesian paths to generate for each target pose.'
"num_cartesian_target_paths_per_target_pose": {
"default": "2",
"description": "Number of Cartesian paths to generate for each target pose.",
},
'cartesian_path_distance_m': {
'default': '0.10',
'description': 'Length of the Cartesian path to set the goal for.'
"cartesian_path_distance_m": {
"default": "0.10",
"description": "Length of the Cartesian path to set the goal for.",
},

# Trajectory Cache
'exact_match_precision': {
"exact_match_precision": {
# Purposely set a relatively high value to make pruning obvious.
'default': '0.0001',
'description': 'Precision for checking if two trajectories are exactly the same.'
"default": "0.0001",
"description": "Precision for checking if two trajectories are exactly the same.",
},
'cartesian_max_step': {
'default': '0.001',
'description': 'Maximum step size for the Cartesian path.'
"cartesian_max_step": {
"default": "0.001",
"description": "Maximum step size for the Cartesian path.",
},
'cartesian_jump_threshold': {
'default': '0.0',
'description': 'Threshold for the jump distance between points in the Cartesian path.'
"cartesian_jump_threshold": {
"default": "0.0",
"description": "Threshold for the jump distance between points in the Cartesian path.",
},
}


def declare_configurable_parameters(parameters):
return [
DeclareLaunchArgument(
param_name,
default_value=param['default'],
description=param['description']
) for param_name, param in parameters.items()
param_name, default_value=param["default"], description=param["description"]
)
for param_name, param in parameters.items()
]


def set_configurable_parameters(parameters):
return {
param_name: LaunchConfiguration(param_name)
for param_name in parameters.keys()
param_name: LaunchConfiguration(param_name) for param_name in parameters.keys()
}


Expand All @@ -114,8 +110,6 @@ def generate_launch_description():
)

return LaunchDescription(
declare_configurable_parameters(configurable_parameters) +
[
trajectory_cache_demo
]
declare_configurable_parameters(configurable_parameters)
+ [trajectory_cache_demo]
)
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@ def generate_launch_description():

# RViz
rviz_config_file = (
get_package_share_directory(
"moveit2_tutorials") + "/launch/trajectory_cache.rviz"
get_package_share_directory("moveit2_tutorials")
+ "/launch/trajectory_cache.rviz"
)
rviz_node = Node(
package="rviz2",
Expand Down Expand Up @@ -97,8 +97,7 @@ def generate_launch_description():
]:
load_controllers += [
ExecuteProcess(
cmd=["ros2 run controller_manager spawner {}".format(
controller)],
cmd=["ros2 run controller_manager spawner {}".format(controller)],
shell=True,
output="screen",
)
Expand Down
18 changes: 12 additions & 6 deletions doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -414,12 +414,18 @@ void vizLegendText(moveit_visual_tools::MoveItVisualTools& visual_tools, const E
static const std::string legend_text = []() {
std::stringstream ss;
ss << "[[LEGEND]]\n";
ss << "TRANSLUCENT:" << HAIR_SPACE << "planner_plans" << "\n";
ss << "GREY:" << HAIR_SPACE << "all_cached_plans" << "\n";
ss << "WHITE:" << HAIR_SPACE << "matchable_cached_plans" << "\n";
ss << "YELLOW:" << HAIR_SPACE << "matched_cached_plans" << "\n";
ss << "GREEN:" << HAIR_SPACE << "best_cached_plan" << "\n";
ss << "RED:" << HAIR_SPACE << "diff_to_trajectory" << "\n";
ss << "TRANSLUCENT:" << HAIR_SPACE << "planner_plans"
<< "\n";
ss << "GREY:" << HAIR_SPACE << "all_cached_plans"
<< "\n";
ss << "WHITE:" << HAIR_SPACE << "matchable_cached_plans"
<< "\n";
ss << "YELLOW:" << HAIR_SPACE << "matched_cached_plans"
<< "\n";
ss << "GREEN:" << HAIR_SPACE << "best_cached_plan"
<< "\n";
ss << "RED:" << HAIR_SPACE << "diff_to_trajectory"
<< "\n";
return ss.str();
}();

Expand Down

0 comments on commit 474b242

Please sign in to comment.