30 #ifndef JOINT_LIMITS_INTERFACE_JOINT_LIMITS_ROSPARAM_H 31 #define JOINT_LIMITS_INTERFACE_JOINT_LIMITS_ROSPARAM_H 81 const std::string limits_namespace =
"joint_limits/" + joint_name;
84 ROS_DEBUG_STREAM(
"No joint limits specification found for joint '" << joint_name <<
85 "' in the parameter server (namespace " << nh.
getNamespace() +
"/" + limits_namespace <<
").");
97 bool has_position_limits =
false;
98 if(limits_nh.
getParam(
"has_position_limits", has_position_limits))
101 double min_pos, max_pos;
102 if (has_position_limits && limits_nh.
getParam(
"min_position", min_pos) && limits_nh.
getParam(
"max_position", max_pos))
109 bool angle_wraparound;
110 if (!has_position_limits && limits_nh.
getParam(
"angle_wraparound", angle_wraparound))
117 bool has_velocity_limits =
false;
118 if(limits_nh.
getParam(
"has_velocity_limits", has_velocity_limits))
122 if (has_velocity_limits && limits_nh.
getParam(
"max_velocity", max_vel))
130 bool has_acceleration_limits =
false;
131 if(limits_nh.
getParam(
"has_acceleration_limits", has_acceleration_limits))
135 if (has_acceleration_limits && limits_nh.
getParam(
"max_acceleration", max_acc))
143 bool has_jerk_limits =
false;
144 if(limits_nh.
getParam(
"has_jerk_limits", has_jerk_limits))
148 if (has_jerk_limits && limits_nh.
getParam(
"max_jerk", max_jerk))
156 bool has_effort_limits =
false;
157 if(limits_nh.
getParam(
"has_effort_limits", has_effort_limits))
161 if (has_effort_limits && limits_nh.
getParam(
"max_effort", max_effort))
200 const std::string limits_namespace =
"joint_limits/" + joint_name;
203 ROS_DEBUG_STREAM(
"No soft joint limits specification found for joint '" << joint_name <<
204 "' in the parameter server (namespace " << nh.
getNamespace() +
"/" + limits_namespace <<
").");
216 bool has_soft_limits;
217 if(limits_nh.
getParam(
"has_soft_limits", has_soft_limits))
219 if(has_soft_limits && limits_nh.
hasParam(
"k_position") && limits_nh.
hasParam(
"k_velocity") && limits_nh.
hasParam(
"soft_lower_limit") && limits_nh.
hasParam(
"soft_upper_limit"))
bool getSoftJointLimits(const std::string &joint_name, const ros::NodeHandle &nh, SoftJointLimits &soft_limits)
Populate a SoftJointLimits instance from the ROS parameter server.
bool getJointLimits(const std::string &joint_name, const ros::NodeHandle &nh, JointLimits &limits)
Populate a JointLimits instance from the ROS parameter server.
const std::string & getNamespace() const
bool has_acceleration_limits
#define ROS_DEBUG_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)