rcll_beacon_sender_node.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *  rcll_fawkes_sim_node.cpp - RCLL simulation access through Fawkes
00003  *
00004  *  Created: Sun May 29 15:36:18 2016
00005  *  Copyright  2016  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU Library General Public License for more details.
00017  *
00018  *  Read the full text in the LICENSE.GPL file in the doc directory.
00019  */
00020 
00021 #include <ros/ros.h>
00022 
00023 #include <rcll_ros_msgs/SendBeaconSignal.h>
00024 
00025 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00026 
00027 geometry_msgs::PoseWithCovarianceStamped last_pose_;
00028 ros::ServiceClient scl_send_beacon_;
00029 
00030 unsigned int seq_num_;
00031 
00032 void cb_pose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
00033 {
00034         last_pose_ = *msg;
00035 }
00036 
00037 void
00038 init_service_client(ros::NodeHandle &n)
00039 {
00040         if (ros::service::waitForService("rcll/send_beacon", ros::Duration(5.0))) {
00041                 ROS_INFO("Got service, creating service client");
00042                 scl_send_beacon_ =
00043                         n.serviceClient<rcll_ros_msgs::SendBeaconSignal>("rcll/send_beacon", /* persistent */ true);
00044         }
00045 }
00046 
00047 void cb_timer(const ros::WallTimerEvent& event, ros::NodeHandle &n)
00048 {
00049         if (! scl_send_beacon_.isValid()) {
00050                 init_service_client(n);
00051         }
00052         if (! scl_send_beacon_.isValid()) {
00053                 ROS_WARN_THROTTLE(30, "Beacon service client disconnected, retrying later");
00054                 return;
00055         }
00056 
00057         rcll_ros_msgs::SendBeaconSignal sbs;
00058         sbs.request.header.seq = ++seq_num_;
00059         ros::WallTime now = ros::WallTime::now();
00060         sbs.request.header.stamp.sec  = now.sec;
00061         sbs.request.header.stamp.nsec = now.nsec;
00062         sbs.request.pose.pose = last_pose_.pose.pose;
00063         sbs.request.pose.header = last_pose_.header;
00064         if (scl_send_beacon_.call(sbs)) {
00065                 if (! sbs.response.ok) {
00066                         ROS_WARN_THROTTLE(30, "Failed to send beacon: %s", sbs.response.error_msg.c_str());
00067                 }
00068         } else {
00069                 ROS_ERROR("Failed to call beacon service");
00070         }
00071 }
00072 
00073 
00074 
00075 int
00076 main(int argc, char **argv)
00077 {
00078         ros::init(argc, argv, "rcll_beacon_sender");
00079 
00080         ros::NodeHandle n;
00081 
00082         seq_num_ = 0;
00083 
00084         init_service_client(n);
00085 
00086         ros::Subscriber sub_pose =
00087                 n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("rcll_sim/amcl_pose", 10, cb_pose);
00088 
00089         ros::WallTimer timer =
00090                 n.createWallTimer(ros::WallDuration(1.0), boost::bind(cb_timer, _1, boost::ref(n)));
00091 
00092   ros::spin();
00093   
00094         return 0;
00095 }


rcll_fawkes_sim
Author(s): Tim Niemueller
autogenerated on Thu Jun 6 2019 17:48:58