Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00029
00030 #ifndef JOINT_LIMITS_INTERFACE_JOINT_LIMITS_ROSPARAM_H
00031 #define JOINT_LIMITS_INTERFACE_JOINT_LIMITS_ROSPARAM_H
00032
00033 #include <string>
00034
00035 #include <ros/ros.h>
00036 #include <joint_limits_interface/joint_limits.h>
00037
00038 namespace joint_limits_interface
00039 {
00040
00075 inline bool getJointLimits(const std::string& joint_name, const ros::NodeHandle& nh, JointLimits& limits)
00076 {
00077
00078 ros::NodeHandle limits_nh;
00079 try
00080 {
00081 const std::string limits_namespace = "joint_limits/" + joint_name;
00082 if (!nh.hasParam(limits_namespace))
00083 {
00084 ROS_DEBUG_STREAM("No joint limits specification found for joint '" << joint_name <<
00085 "' in the parameter server (namespace " << nh.getNamespace() + "/" + limits_namespace << ").");
00086 return false;
00087 }
00088 limits_nh = ros::NodeHandle(nh, limits_namespace);
00089 }
00090 catch(const ros::InvalidNameException& ex)
00091 {
00092 ROS_ERROR_STREAM(ex.what());
00093 return false;
00094 }
00095
00096
00097 bool has_position_limits = false;
00098 if(limits_nh.getParam("has_position_limits", has_position_limits))
00099 {
00100 if (!has_position_limits) {limits.has_position_limits = false;}
00101 double min_pos, max_pos;
00102 if (has_position_limits && limits_nh.getParam("min_position", min_pos) && limits_nh.getParam("max_position", max_pos))
00103 {
00104 limits.has_position_limits = true;
00105 limits.min_position = min_pos;
00106 limits.max_position = max_pos;
00107 }
00108
00109 bool angle_wraparound;
00110 if (!has_position_limits && limits_nh.getParam("angle_wraparound", angle_wraparound))
00111 {
00112 limits.angle_wraparound = angle_wraparound;
00113 }
00114 }
00115
00116
00117 bool has_velocity_limits = false;
00118 if(limits_nh.getParam("has_velocity_limits", has_velocity_limits))
00119 {
00120 if (!has_velocity_limits) {limits.has_velocity_limits = false;}
00121 double max_vel;
00122 if (has_velocity_limits && limits_nh.getParam("max_velocity", max_vel))
00123 {
00124 limits.has_velocity_limits = true;
00125 limits.max_velocity = max_vel;
00126 }
00127 }
00128
00129
00130 bool has_acceleration_limits = false;
00131 if(limits_nh.getParam("has_acceleration_limits", has_acceleration_limits))
00132 {
00133 if (!has_acceleration_limits) {limits.has_acceleration_limits = false;}
00134 double max_acc;
00135 if (has_acceleration_limits && limits_nh.getParam("max_acceleration", max_acc))
00136 {
00137 limits.has_acceleration_limits = true;
00138 limits.max_acceleration = max_acc;
00139 }
00140 }
00141
00142
00143 bool has_jerk_limits = false;
00144 if(limits_nh.getParam("has_jerk_limits", has_jerk_limits))
00145 {
00146 if (!has_jerk_limits) {limits.has_jerk_limits = false;}
00147 double max_jerk;
00148 if (has_jerk_limits && limits_nh.getParam("max_jerk", max_jerk))
00149 {
00150 limits.has_jerk_limits = true;
00151 limits.max_jerk = max_jerk;
00152 }
00153 }
00154
00155
00156 bool has_effort_limits = false;
00157 if(limits_nh.getParam("has_effort_limits", has_effort_limits))
00158 {
00159 if (!has_effort_limits) {limits.has_effort_limits = false;}
00160 double max_effort;
00161 if (has_effort_limits && limits_nh.getParam("max_effort", max_effort))
00162 {
00163 limits.has_effort_limits = true;
00164 limits.max_effort = max_effort;
00165 }
00166 }
00167
00168 return true;
00169 }
00170
00171 }
00172
00173 #endif