Skip to content
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

Jump to bottom

Get parameter on initialize #893

Closed
wants to merge 10 commits into from
Closed

Get parameter on initialize #893

Changes from 2 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
e4cf119
Get parameter on initialize
galou Dec 6, 2021
a7e836a
Move defaults closer to where they are overwritten
galou Dec 6, 2021
05c7675
Satisfy clang-format
galou Dec 6, 2021
a14a2f4
Merge branch 'main' into fix_duration_monitoring
tylerjw Dec 7, 2021
f1d8021
Merge branch 'main' into fix_duration_monitoring
galou Dec 9, 2021
2c334d0
Merge branch 'fix_duration_monitoring' of https://github.com/galou/mo…
galou Dec 9, 2021
61c44fd
Merge branch 'main' into fix_duration_monitoring
tylerjw Dec 13, 2021
25edd14
Merge branch 'main' into fix_duration_monitoring
tylerjw Dec 16, 2021
eddc3bb
Merge branch 'main' into fix_duration_monitoring
tylerjw Dec 22, 2021
7b23c83
Merge branch 'main' into fix_duration_monitoring
tylerjw Dec 23, 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
22 changes: 14 additions & 8 deletions moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,13 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.trajectory_e
const std::string TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC = "trajectory_execution_event";

static const auto DEFAULT_CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1);
static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5; // allow 0.5s more than the expected execution time
constexpr double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5; // allow 0.5s more than the expected execution time
// before triggering a trajectory cancel (applied
// after scaling)
static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING =
1.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 1)
// allow the execution of a trajectory to take more time than expected (scaled by a value > 1)
constexpr double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING = 1.1;

constexpr double DEFAULT_ALLOWED_START_TOLERANCE = 0.01;

TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node,
const moveit::core::RobotModelConstPtr& robot_model,
Expand Down Expand Up @@ -93,14 +95,9 @@ void TrajectoryExecutionManager::initialize()
current_context_ = -1;
last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
run_continuous_execution_thread_ = true;
execution_duration_monitoring_ = true;
execution_velocity_scaling_ = 1.0;
allowed_start_tolerance_ = 0.01;
wait_for_trajectory_completion_ = true;

allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;

// load controller-specific values for allowed_execution_duration_scaling and allowed_goal_duration_margin
loadControllerParams();

Expand Down Expand Up @@ -181,10 +178,19 @@ void TrajectoryExecutionManager::initialize()
EXECUTION_EVENT_TOPIC, 100, std::bind(&TrajectoryExecutionManager::receiveEvent, this, std::placeholders::_1),
options);

execution_duration_monitoring_ = true;
controller_mgr_node_->get_parameter("trajectory_execution.execution_duration_monitoring",
execution_duration_monitoring_);
Copy link
Member

@AndyZe AndyZe Dec 6, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This generally makes sense to me! I would like to see L96 & 98 moved closer to these lines of code so it's more clear that they are the default and they might get overwritten.

Suggested change
execution_duration_monitoring_);
execution_duration_monitoring_ = true;
controller_mgr_node_->get_parameter("trajectory_execution.execution_duration_monitoring",
execution_duration_monitoring_);
controller_mgr_node_->get_parameter("trajectory_execution.allowed_execution_duration_scaling",
allowed_execution_duration_scaling_);
controller_mgr_node_->get_parameter("trajectory_execution.allowed_goal_duration_margin",
allowed_start_tolerance_ = 0.01;
controller_mgr_node_->get_parameter("trajectory_execution.allowed_start_tolerance", allowed_start_tolerance_);

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

get_parameter_or() is probably a better way to set a default value if none is defined.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the regular get_parameter will throw an exception if it is not declared first (or someone didn't use the setting that decades everything in your yaml file and it was in the yaml file).


allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING;
controller_mgr_node_->get_parameter("trajectory_execution.allowed_execution_duration_scaling",
allowed_execution_duration_scaling_);

allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN;
controller_mgr_node_->get_parameter("trajectory_execution.allowed_goal_duration_margin",
allowed_goal_duration_margin_);

allowed_start_tolerance_ = DEFAULT_ALLOWED_START_TOLERANCE;
controller_mgr_node_->get_parameter("trajectory_execution.allowed_start_tolerance", allowed_start_tolerance_);

if (manage_controllers_)
Expand Down