45 if (detection_type ==
"rc_april_tag_detect" || detection_type ==
"rc_qr_code_detect")
51 throw std::runtime_error(
"Acceptable detection types are \"rc_april_tag_detect\" and \"rc_qr_code_detect\"");
71 catch (
const std::exception& ex)
73 ROS_FATAL(
"Exception during destruction of RosTagdetectClient: %s", ex.what());
77 ROS_FATAL(
"Exception during destruction of RosTagdetectClient");
91 template <
typename Request,
typename Response>
97 const auto j_res =
rest_helper_->servicePutRequest(name, j_req);
103 ROS_ERROR(
"This rc_visard firmware does not support \"%s\"", ex.what());
104 res.return_code.value = -8;
105 res.return_code.message =
"Not available in this firmware version";
108 catch (
const std::exception& ex)
111 res.return_code.value = -2;
112 res.return_code.message = ex.what();
120 if (success && res.return_code.value >= 0 &&
visualizer_)
132 ROS_ERROR(
"Cannot execute detect when continuous mode is enabled.");
133 res.return_code.value = -8;
134 res.return_code.message =
"Cannot execute detect when continuous mode is enabled.";
149 response.success =
true;
154 StartContinuousDetectionResponse& response)
157 std_srvs::TriggerRequest request;
158 std_srvs::TriggerResponse response;
166 ROS_INFO(
"Entering continuous mode...");
168 const double rate = 1.0;
174 DetectTagsRequest req;
175 req.tags = request.tags;
176 req.pose_frame = request.pose_frame;
177 req.robot_pose = request.robot_pose;
178 DetectTagsResponse
res;
188 std::this_thread::sleep_for(std::chrono::milliseconds(10));
192 ROS_INFO(
"Stopping continuous mode...");
196 response.success =
true;
213 for (
const auto&
param : params)
216 if (
name ==
"quality")
218 cfg.quality =
param[
"value"];
220 else if (
name ==
"detect_inverted_tags")
223 cfg.detect_inverted_tags =
param[
"value"];
225 else if (
name ==
"forget_after_n_detections")
227 cfg.forget_after_n_detections =
param[
"value"];
229 else if (
name ==
"max_corner_distance")
231 cfg.max_corner_distance =
param[
"value"];
233 else if (
name ==
"use_cached_images")
235 cfg.use_cached_images =
param[
"value"];
242 rc_tagdetect_client::TagDetectConfig cfg;
251 nh_.
param(
"quality", cfg.quality, cfg.quality);
252 nh_.
param(
"detect_inverted_tags", cfg.detect_inverted_tags, cfg.detect_inverted_tags);
253 nh_.
param(
"forget_after_n_detections", cfg.forget_after_n_detections, cfg.forget_after_n_detections);
254 nh_.
param(
"max_corner_distance", cfg.max_corner_distance, cfg.max_corner_distance);
255 nh_.
param(
"use_cached_images", cfg.use_cached_images, cfg.use_cached_images);
259 nh_.
setParam(
"detect_inverted_tags", cfg.detect_inverted_tags);
260 nh_.
setParam(
"forget_after_n_detections", cfg.forget_after_n_detections);
261 nh_.
setParam(
"max_corner_distance", cfg.max_corner_distance);
262 nh_.
setParam(
"use_cached_images", cfg.use_cached_images);
265 using ReconfServer = dynamic_reconfigure::Server<rc_tagdetect_client::TagDetectConfig>;
266 server_ = std::unique_ptr<ReconfServer>(
new ReconfServer(
nh_));
273 json js_params, js_param;
274 js_param[
"name"] =
"quality";
275 js_param[
"value"] = config.quality;
276 js_params.push_back(js_param);
279 js_param[
"name"] =
"detect_inverted_tags";
280 js_param[
"value"] = config.detect_inverted_tags;
281 js_params.push_back(js_param);
285 ROS_WARN(
"\"detect_inverted_tags\" only available in newer software versions.");
286 config.detect_inverted_tags =
false;
288 js_param[
"name"] =
"forget_after_n_detections";
289 js_param[
"value"] = config.forget_after_n_detections;
290 js_params.push_back(js_param);
291 js_param[
"name"] =
"max_corner_distance";
292 js_param[
"value"] = config.max_corner_distance;
293 js_params.push_back(js_param);
294 js_param[
"name"] =
"use_cached_images";
295 js_param[
"value"] = config.use_cached_images;
296 js_params.push_back(js_param);
298 if (config.publish_visualization)
std::unique_ptr< dynamic_reconfigure::Server< rc_tagdetect_client::TagDetectConfig > > server_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val)
bool detect(rc_tagdetect_client::DetectTagsRequest &req, rc_tagdetect_client::DetectTagsResponse &response)
void publish(const boost::shared_ptr< M > &message) const
ros::ServiceServer srv_start_continuous_detection_
bool startContinousDetection(rc_tagdetect_client::StartContinuousDetectionRequest &request, rc_tagdetect_client::StartContinuousDetectionResponse &response)
RosTagdetectClient(const std::string &host, const ros::NodeHandle &nh, const std::string &detection_type)
void advertiseServicesAndTopics()
std::tuple< size_t, size_t, size_t > image_version_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void dynamicReconfigureCallback(rc_tagdetect_client::TagDetectConfig &config, uint32_t)
std::unique_ptr< rc_rest_api::RestHelper > rest_helper_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
std::thread continuous_mode_thread_
ros::ServiceServer srv_stop_continuous_detection_
ros::Publisher detections_pub_
std::atomic_bool stop_continuous_mode_thread_
std::unique_ptr< rc_tagdetect_client::Visualization > visualizer_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool stopContinousDetection(std_srvs::TriggerRequest &request, std_srvs::TriggerResponse &response)
void paramsToCfg(const json ¶ms, TagDetectConfig &cfg)
ros::ServiceServer srv_detect_
bool detectService(rc_tagdetect_client::DetectTagsRequest &request, rc_tagdetect_client::DetectTagsResponse &response)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
bool callService(const std::string &name, const Request &req, Response &res)