-
Notifications
You must be signed in to change notification settings - Fork 333
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.
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
[JTC] Process tolerances sent with action goal #716
Changes from 28 commits
c3acc81
c168ab3
0328c94
880b87b
57d6b32
ac0e871
2c717d0
31afb42
16a8828
6a95acb
4a3ba8b
bf6d034
22543a9
fcaa1b9
534746c
06400fa
7e8fa9f
ae5efec
cb0e733
597a2b8
88666eb
7cc1244
6d21d3a
a5d548c
e48ca7b
4f1b755
ec87dc0
54d279c
1a592ab
2bdc0c2
36ecdda
34ca300
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -30,12 +30,14 @@ | |
#ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ | ||
#define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ | ||
|
||
#include <limits> | ||
#include <vector> | ||
|
||
#include "control_msgs/action/follow_joint_trajectory.hpp" | ||
#include "joint_trajectory_controller_parameters.hpp" | ||
|
||
#include "rclcpp/node.hpp" | ||
#include "rclcpp/time.hpp" | ||
|
||
namespace joint_trajectory_controller | ||
{ | ||
|
@@ -95,6 +97,8 @@ SegmentTolerances get_segment_tolerances(Params const & params) | |
|
||
SegmentTolerances tolerances; | ||
tolerances.goal_time_tolerance = constraints.goal_time; | ||
auto logger = rclcpp::get_logger("tolerance"); | ||
RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); | ||
|
||
// State and goal state tolerances | ||
tolerances.state_tolerance.resize(n_joints); | ||
|
@@ -106,16 +110,219 @@ SegmentTolerances get_segment_tolerances(Params const & params) | |
tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; | ||
tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; | ||
|
||
auto logger = rclcpp::get_logger("tolerance"); | ||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); | ||
logger, "%s %f", (joint + ".trajectory.position").c_str(), | ||
tolerances.state_tolerance[i].position); | ||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); | ||
logger, "%s %f", (joint + ".goal.position").c_str(), | ||
tolerances.goal_state_tolerance[i].position); | ||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".goal.velocity").c_str(), | ||
tolerances.goal_state_tolerance[i].velocity); | ||
} | ||
|
||
return tolerances; | ||
} | ||
|
||
/** | ||
* \brief Populate trajectory segment tolerances using data from an action goal. | ||
* | ||
* \param default_tolerances The default tolerances to use if the action goal does not specify any. | ||
* \param goal The new action goal | ||
* \param params The ROS Parameters | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I am not sure why do we need ROS parameters because the default tolerances should be added through parameters, isn't it? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The only field we need is the params.joints actually, I can change it to have a std::vector as argument instead? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. done |
||
* \return Trajectory segment tolerances. | ||
*/ | ||
SegmentTolerances get_segment_tolerances( | ||
const SegmentTolerances & default_tolerances, | ||
const control_msgs::action::FollowJointTrajectory::Goal & goal, const Params & params) | ||
{ | ||
SegmentTolerances active_tolerances(default_tolerances); | ||
|
||
active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); | ||
auto logger = rclcpp::get_logger("tolerance"); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. It would be great to have a static object for this, as mentioned above because we are creating objects all the time. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. see above |
||
RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); | ||
|
||
// from | ||
// https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg | ||
// There are two special values for tolerances: | ||
// * 0 - The tolerance is unspecified and will remain at whatever the default is | ||
// * -1 - The tolerance is "erased". | ||
// If there was a default, the joint will be allowed to move without restriction. | ||
constexpr double ERASE_VALUE = -1.0; | ||
auto is_erase_value = [](double value) | ||
{ return fabs(value - ERASE_VALUE) < std::numeric_limits<float>::epsilon(); }; | ||
|
||
// State and goal state tolerances | ||
for (auto joint_tol : goal.path_tolerance) | ||
{ | ||
auto const joint = joint_tol.name; | ||
// map joint names from goal to active_tolerances | ||
auto it = std::find(params.joints.begin(), params.joints.end(), joint); | ||
if (it == params.joints.end()) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.path_tolerance does not exist. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
auto i = std::distance(params.joints.cbegin(), it); | ||
if (joint_tol.position > 0.0) | ||
{ | ||
active_tolerances.state_tolerance[i].position = joint_tol.position; | ||
} | ||
else if (is_erase_value(joint_tol.position)) | ||
{ | ||
active_tolerances.state_tolerance[i].position = 0.0; | ||
} | ||
else if (joint_tol.position < 0.0) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.path_tolerance has a invalid position tolerance. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
|
||
if (joint_tol.velocity > 0.0) | ||
{ | ||
active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; | ||
} | ||
else if (is_erase_value(joint_tol.velocity)) | ||
{ | ||
active_tolerances.state_tolerance[i].velocity = 0.0; | ||
} | ||
else if (joint_tol.velocity < 0.0) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.path_tolerance has a invalid velocity tolerance. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
|
||
if (joint_tol.acceleration > 0.0) | ||
{ | ||
active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; | ||
} | ||
else if (is_erase_value(joint_tol.acceleration)) | ||
{ | ||
active_tolerances.state_tolerance[i].acceleration = 0.0; | ||
} | ||
else if (joint_tol.acceleration < 0.0) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.path_tolerance has a invalid acceleration tolerance. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
|
||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".state_tolerance.position").c_str(), | ||
active_tolerances.state_tolerance[i].position); | ||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), | ||
active_tolerances.state_tolerance[i].velocity); | ||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), | ||
active_tolerances.state_tolerance[i].acceleration); | ||
} | ||
for (auto goal_tol : goal.goal_tolerance) | ||
{ | ||
auto const joint = goal_tol.name; | ||
// map joint names from goal to active_tolerances | ||
auto it = std::find(params.joints.begin(), params.joints.end(), joint); | ||
if (it == params.joints.end()) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.goal_tolerance does not exist. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
auto i = std::distance(params.joints.cbegin(), it); | ||
if (goal_tol.position > 0.0) | ||
{ | ||
active_tolerances.goal_state_tolerance[i].position = goal_tol.position; | ||
} | ||
else if (is_erase_value(goal_tol.position)) | ||
{ | ||
active_tolerances.goal_state_tolerance[i].position = 0.0; | ||
} | ||
else if (goal_tol.position < 0.0) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.goal_tolerance has a invalid position tolerance. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
|
||
if (goal_tol.velocity > 0.0) | ||
{ | ||
active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; | ||
} | ||
else if (is_erase_value(goal_tol.velocity)) | ||
{ | ||
active_tolerances.goal_state_tolerance[i].velocity = 0.0; | ||
} | ||
else if (goal_tol.velocity < 0.0) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.goal_tolerance has a invalid velocity tolerance. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
|
||
if (goal_tol.acceleration > 0.0) | ||
{ | ||
active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; | ||
} | ||
else if (is_erase_value(goal_tol.acceleration)) | ||
{ | ||
active_tolerances.goal_state_tolerance[i].acceleration = 0.0; | ||
} | ||
else if (goal_tol.acceleration < 0.0) | ||
{ | ||
RCLCPP_ERROR( | ||
logger, "%s", | ||
("joint '" + joint + | ||
"' specified in goal.goal_tolerance has a invalid acceleration tolerance. " | ||
"Using default tolerances.") | ||
.c_str()); | ||
return default_tolerances; | ||
} | ||
|
||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), | ||
active_tolerances.goal_state_tolerance[i].position); | ||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), | ||
active_tolerances.goal_state_tolerance[i].velocity); | ||
RCLCPP_DEBUG( | ||
logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), | ||
active_tolerances.goal_state_tolerance[i].acceleration); | ||
} | ||
|
||
return active_tolerances; | ||
} | ||
|
||
/** | ||
* \param state_error State error to check. | ||
* \param joint_idx Joint index for the state error | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Maybe might be better to have a logger object and initialize child tolerances here of the JTC node.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I haven't had a better idea as passing the JTC logger as argument to the
get_segment_tolerances
methods. There is no persistent class with a configuration/constructor method to pass the logger only once.