40 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoAction.h> 41 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoData.h> 42 #include <pr2_gripper_sensor_msgs/PR2GripperForceServoCommand.h> 43 #include <std_srvs/Empty.h> 62 node_.
advertise<pr2_gripper_sensor_msgs::PR2GripperForceServoCommand>(
"force_servo", 1);
103 if (has_active_goal_)
105 bool should_abort =
false;
109 ROS_WARN(
"Aborting goal because we have never heard a controller state message.");
114 ROS_WARN(
"Aborting goal because we haven't heard from the controller in %.3lf seconds",
122 has_active_goal_ =
false;
130 if (has_active_goal_)
134 has_active_goal_ =
false;
139 has_active_goal_ =
true;
153 if (active_goal_ == gh)
156 std_srvs::Empty::Request req;
157 std_srvs::Empty::Response
resp;
166 has_active_goal_ =
false;
174 last_controller_state_ = msg;
177 if (!has_active_goal_)
180 pr2_gripper_sensor_msgs::PR2GripperForceServoGoal goal;
181 goal.command = active_goal_.
getGoal()->command;
183 pr2_gripper_sensor_msgs::PR2GripperForceServoFeedback feedback;
184 feedback.data = *msg;
186 pr2_gripper_sensor_msgs::PR2GripperForceServoResult result;
192 if(feedback.data.stamp.toSec() < action_start_time.
toSec()+dT ){}
195 else if(feedback.data.rtstate.realtime_controller_state == 4)
198 if ( feedback.data.force_achieved )
201 has_active_goal_ =
false;
207 has_active_goal_ =
false;
214 int main(
int argc,
char** argv)
216 ros::init(argc, argv,
"force_servo_node");
void publishFeedback(const Feedback &feedback)
GAS::GoalHandle GoalHandle
double stall_velocity_threshold_
ros::Subscriber sub_controller_state_
void publish(const boost::shared_ptr< M > &message) const
void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperForceServoDataConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Timer watchdog_timer_
bool call(const std::string &service_name, MReq &req, MRes &res)
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cancelCB(GoalHandle gh)
boost::shared_ptr< const Goal > getGoal() const
void watchdog(const ros::TimerEvent &e)
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL void spin(Spinner &spinner)
void setAccepted(const std::string &text=std::string(""))
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ros::Time last_movement_time_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void goalCB(GoalHandle gh)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
ros::Publisher pub_controller_command_
Pr2GripperForceServo(ros::NodeHandle &n)
actionlib::ActionServer< pr2_gripper_sensor_msgs::PR2GripperForceServoAction > GAS
pr2_gripper_sensor_msgs::PR2GripperForceServoDataConstPtr last_controller_state_
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
ros::Time action_start_time