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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/pick_place/pick_place_params.h>
00038 #include <dynamic_reconfigure/server.h>
00039 #include <moveit_ros_manipulation/PickPlaceDynamicReconfigureConfig.h>
00040
00041 namespace pick_place
00042 {
00043
00044 namespace
00045 {
00046 using namespace moveit_ros_manipulation;
00047
00048 class DynamicReconfigureImpl
00049 {
00050 public:
00051
00052 DynamicReconfigureImpl() : dynamic_reconfigure_server_(ros::NodeHandle("~/pick_place"))
00053 {
00054 dynamic_reconfigure_server_.setCallback(boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
00055 }
00056
00057 const PickPlaceParams& getParams() const
00058 {
00059 return params_;
00060 }
00061
00062 private:
00063 PickPlaceParams params_;
00064
00065 void dynamicReconfigureCallback(PickPlaceDynamicReconfigureConfig &config, uint32_t level)
00066 {
00067 params_.max_goal_count_ = config.max_attempted_states_per_pose;
00068 params_.max_fail_ = config.max_consecutive_fail_attempts;
00069 params_.max_step_ = config.cartesian_motion_step_size;
00070 params_.jump_factor_ = config.jump_factor;
00071 }
00072
00073 dynamic_reconfigure::Server<PickPlaceDynamicReconfigureConfig> dynamic_reconfigure_server_;
00074 };
00075
00076 }
00077 }
00078
00079 pick_place::PickPlaceParams::PickPlaceParams() : max_goal_count_(5),
00080 max_fail_(3),
00081 max_step_(0.02),
00082 jump_factor_(2.0)
00083 {
00084 }
00085
00086 const pick_place::PickPlaceParams& pick_place::GetGlobalPickPlaceParams() {
00087 static DynamicReconfigureImpl PICK_PLACE_PARAMS;
00088 return PICK_PLACE_PARAMS.getParams();
00089 }