void unadvertise() { if (!unadvertised_) { unadvertised_ = true; srv_server_.shutdown(); client_map_.clear(); } }
bool caseFlipUnadvertise(roscpp::TestStringString::Request &req, roscpp::TestStringString::Response &res, ros::ServiceServer& srv) { caseFlip(req, res); srv.shutdown(); ros::Duration(2).sleep(); return true; }
int main(int argc, char** argv) { ros::init(argc, argv, "goal_server_cu"); ros::NodeHandle nh; ros::Rate loop_rate(100); // load globals from parameter server double param_input; bool given_goal = false; if(ros::param::get("goal_server_cu/goal_pose_x_init", param_input)) { goal_pose_x_init = (float)param_input; given_goal = true; } if(ros::param::get("goal_server_cu/goal_pose_y_init", param_input)) { goal_pose_y_init = (float)param_input; given_goal = true; } if(ros::param::get("goal_server_cu/goal_pose_z_init", param_input)) { goal_pose_z_init = (float)param_input; // z is basically unused given_goal = true; } if(ros::param::get("goal_server_cu/goal_pose_theta_init", param_input)) { goal_pose_theta_init = (float)param_input; given_goal = true; } // if the goal was sent in, then init goal pose if(given_goal) { printf("goal recieved at startup: [%f %f %f] \n", goal_pose_x_init, goal_pose_y_init, goal_pose_theta_init); goal = make_pose(goal_pose_x_init, goal_pose_y_init, goal_pose_z_init, goal_pose_theta_init); } // set up publisher goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/cu/goal_cu", 1); // set up subscribers reset_goal_sub = nh.subscribe("/cu/reset_goal_cu", 1, reset_goal_callback); // set up service servers get_goal_srv = nh.advertiseService("/cu/get_goal_cu", get_goal_callback); while (ros::ok()) { //printf(" This is the goal server \n"); //print_pose(goal); publish_goal(); ros::spinOnce(); loop_rate.sleep(); } // destroy publisher goal_pub.shutdown(); reset_goal_sub.shutdown(); get_goal_srv.shutdown(); destroy_pose(goal); }