40 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactAction.h> 41 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactData.h> 42 #include <pr2_gripper_sensor_msgs/PR2GripperFindContactCommand.h> 43 #include <std_srvs/Empty.h> 62 node_.
advertise<pr2_gripper_sensor_msgs::PR2GripperFindContactCommand>(
"find_contact", 1);
101 bool should_abort =
false;
105 ROS_WARN(
"Aborting goal because we have never heard a controller state message.");
110 ROS_WARN(
"Aborting goal because we haven't heard from the controller in %.3lf seconds",
118 has_active_goal_ =
false;
126 if (has_active_goal_)
130 has_active_goal_ =
false;
135 has_active_goal_ =
true;
139 if(active_goal_.
getGoal()->command.zero_fingertip_sensors)
141 std_srvs::Empty::Request req;
142 std_srvs::Empty::Response
resp;
160 if (active_goal_ == gh)
163 std_srvs::Empty::Request req;
164 std_srvs::Empty::Response
resp;
173 has_active_goal_ =
false;
181 last_controller_state_ = msg;
184 if (!has_active_goal_)
187 pr2_gripper_sensor_msgs::PR2GripperFindContactGoal goal;
188 goal.command = active_goal_.
getGoal()->command;
190 pr2_gripper_sensor_msgs::PR2GripperFindContactFeedback feedback;
191 feedback.data = *msg;
193 pr2_gripper_sensor_msgs::PR2GripperFindContactResult result;
198 if(feedback.data.stamp.toSec() < action_start_time.
toSec()+dT ){}
202 else if(feedback.data.rtstate.realtime_controller_state == 5 || feedback.data.rtstate.realtime_controller_state == 3)
204 if(feedback.data.contact_conditions_met)
207 has_active_goal_ =
false;
212 has_active_goal_ =
false;
219 int main(
int argc,
char** argv)
221 ros::init(argc, argv,
"find_contact_node");
void publishFeedback(const Feedback &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())
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)
boost::shared_ptr< const Goal > getGoal() const
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
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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)