40 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoAction.h> 41 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoData.h> 42 #include <pr2_gripper_sensor_msgs/PR2GripperSlipServoCommand.h> 43 #include <std_srvs/Empty.h> 67 node_.
advertise<pr2_gripper_sensor_msgs::PR2GripperSlipServoCommand>(
"slip_servo", 1);
107 if (has_active_goal_)
109 bool should_abort =
false;
113 ROS_WARN(
"Aborting goal because we have never heard a controller state message.");
118 ROS_WARN(
"Aborting goal because we haven't heard from the controller in %.3lf seconds",
126 has_active_goal_ =
false;
134 if (has_active_goal_)
138 has_active_goal_ =
false;
143 has_active_goal_ =
true;
157 if (active_goal_ == gh)
160 std_srvs::Empty::Request req;
161 std_srvs::Empty::Response
resp;
170 has_active_goal_ =
false;
178 last_controller_state_ = msg;
181 if (!has_active_goal_)
184 pr2_gripper_sensor_msgs::PR2GripperSlipServoGoal goal;
185 goal.command = active_goal_.
getGoal()->command;
187 pr2_gripper_sensor_msgs::PR2GripperSlipServoFeedback feedback;
188 feedback.data = *msg;
190 pr2_gripper_sensor_msgs::PR2GripperSlipServoResult result;
195 if(feedback.data.stamp.toSec() < action_start_time.
toSec()+dT ){}
198 else if(feedback.data.rtstate.realtime_controller_state == 6)
202 has_active_goal_ =
false;
209 int main(
int argc,
char** argv)
211 ros::init(argc, argv,
"slip_servo_node");
void publishFeedback(const Feedback &feedback)
GAS::GoalHandle GoalHandle
Pr2GripperSlipServo(ros::NodeHandle &n)
pr2_gripper_sensor_msgs::PR2GripperSlipServoDataConstPtr last_controller_state_
void publish(const boost::shared_ptr< M > &message) const
void watchdog(const ros::TimerEvent &e)
actionlib::ActionServer< pr2_gripper_sensor_msgs::PR2GripperSlipServoAction > GAS
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool call(const std::string &service_name, MReq &req, MRes &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
double stall_velocity_threshold_
boost::shared_ptr< const Goal > getGoal() const
void goalCB(GoalHandle gh)
ros::Publisher pub_controller_command_
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(""))
int main(int argc, char **argv)
void controllerStateCB(const pr2_gripper_sensor_msgs::PR2GripperSlipServoDataConstPtr &msg)
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)
ros::Timer watchdog_timer_
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
ros::Time action_start_time
ros::Subscriber sub_controller_state_
void cancelCB(GoalHandle gh)