8 #include <visualization_msgs/Marker.h> 10 int main(
int argc,
char **argv) {
20 double goal_x,goal_y,goal_z,goal_tol;
21 std::string moving_frame,sys_cmd;
22 nodeHandle.
getParam(
"goal_x", goal_x);
23 nodeHandle.
getParam(
"goal_y", goal_y);
24 nodeHandle.
getParam(
"goal_z", goal_z);
25 nodeHandle.
getParam(
"goal_tol", goal_tol);
26 nodeHandle.
getParam(
"moving_frame", moving_frame);
27 nodeHandle.
getParam(
"sys_cmd", sys_cmd);
31 #ifdef DEBUG_REACH_EVENT 35 visualization_msgs::Marker marker;
36 marker.header.frame_id =
"map";
40 marker.type = visualization_msgs::Marker::SPHERE;
41 marker.action = visualization_msgs::Marker::ADD;
42 marker.pose.position.x = goal_x;
43 marker.pose.position.y = goal_y;
44 marker.pose.position.z = goal_z;
45 marker.pose.orientation.x = 0.0;
46 marker.pose.orientation.y = 0.0;
47 marker.pose.orientation.z = 0.0;
48 marker.pose.orientation.w = 1.0;
49 marker.scale.x = 0.05;
50 marker.scale.y = 0.05;
51 marker.scale.z = 0.05;
67 if ((dist <= goal_tol)&&(!inPose)) {
69 #ifdef DEBUG_REACH_EVENT 72 FILE *process=popen(sys_cmd.c_str(),
"r");
73 if (process!=NULL)
ROS_INFO(
"System command done");
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
bool getParam(const std::string &key, std::string &s) const
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()