19 struct BadArgumentsError {};
30 if (goal.command.position > params.
max_gap_ || goal.command.position < params.
min_gap_)
32 ROS_WARN(
"Goal gripper gap size is out of range(%f to %f): %f m",
34 throw BadArgumentsError();
39 ROS_WARN(
"Goal gripper effort out of range (%f to %f N): %f N",
41 throw BadArgumentsError();
47 result.rPR =
static_cast<uint8_t>((params.
max_gap_ - goal.command.position) / dist_per_tick);
48 result.rFR =
static_cast<uint8_t>((goal.command.max_effort - params.
min_effort_) / eff_per_tick);
50 ROS_INFO(
"Setting goal position register to %hhu", result.rPR);
66 result.position = input.gPO * dist_per_tick + params.
min_gap_;
67 result.effort = input.gCU * eff_per_tick + params.
min_effort_;
68 result.stalled = input.gOBJ == 0x1 || input.gOBJ == 0x2;
69 result.reached_goal = input.gPO == goal_pos;
78 return registerStateToResultT<GripperCommandResult>(input, params, goal_pos);
84 return registerStateToResultT<GripperCommandFeedback>(input, params, goal_pos);
94 , as_(nh_, name, false)
96 , gripper_params_(params)
112 ROS_WARN(
"%s could not accept goal because the gripper is not yet active",
action_name_.c_str());
128 catch (BadArgumentsError& e)
control_msgs::GripperCommandResult GripperCommandResult
boost::shared_ptr< const Goal > acceptNewGoal()
void publishFeedback(const FeedbackConstPtr &feedback)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
robotiq_2f_gripper_control::Robotiq2FGripper_robot_output GripperOutput
void analysisCB(const GripperInput::ConstPtr &msg)
ros::Subscriber state_sub_
GripperInput current_reg_state_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
robotiq_2f_gripper_control::Robotiq2FGripper_robot_input GripperInput
GripperOutput goal_reg_state_
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
control_msgs::GripperCommandFeedback GripperCommandFeedback
actionlib::SimpleActionServer< control_msgs::GripperCommandAction > as_
void registerPreemptCallback(boost::function< void()> cb)
bool isPreemptRequested()
Robotiq2FGripperActionServer(const std::string &name, const Robotiq2FGripperParams ¶ms)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
control_msgs::GripperCommandGoal GripperCommandGoal
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void registerGoalCallback(boost::function< void()> cb)
Structure containing the parameters necessary to translate GripperCommand actions to register-based c...
Robotiq2FGripperParams gripper_params_