rc_hand_eye_calibration_client.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018-2020 Roboception GmbH
3  *
4  * Author: Felix Endres
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright notice,
10  * this list of conditions and the following disclaimer.
11  *
12  * 2. Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its contributors
17  * may be used to endorse or promote products derived from this software without
18  * specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef RC_HAND_EYE_CALIBRATION_CLIENT_H
34 #define RC_HAND_EYE_CALIBRATION_CLIENT_H
35 
36 #include <rc_hand_eye_calibration_client/SetCalibrationPose.h>
37 #include <rc_hand_eye_calibration_client/SetCalibration.h>
38 #include <rc_hand_eye_calibration_client/Calibration.h>
39 #include <rc_hand_eye_calibration_client/hand_eye_calibrationConfig.h>
40 #include <rc_hand_eye_calibration_client/Trigger.h>
41 
42 #include <ros/ros.h>
43 #include <dynamic_reconfigure/server.h>
44 
47 #include <geometry_msgs/TransformStamped.h>
48 
49 #include <memory>
50 
51 #include "rest_helper.h"
52 
54 {
56 {
57 public:
58  HandEyeCalibClient(std::string host, ros::NodeHandle nh);
59 
60 private:
61  template <typename Request, typename Response>
62  bool callService(const std::string& name, const Request& req, Response& res);
63 
67  bool calibSrv(rc_hand_eye_calibration_client::CalibrationRequest& req,
68  rc_hand_eye_calibration_client::CalibrationResponse& res);
69 
71  bool getCalibResultSrv(rc_hand_eye_calibration_client::CalibrationRequest& req,
72  rc_hand_eye_calibration_client::CalibrationResponse& res);
73 
77 
79  bool saveSrv(rc_hand_eye_calibration_client::TriggerRequest& req,
80  rc_hand_eye_calibration_client::TriggerResponse& res);
81 
83  bool resetSrv(rc_hand_eye_calibration_client::TriggerRequest& req,
84  rc_hand_eye_calibration_client::TriggerResponse& res);
85 
87  bool removeSrv(rc_hand_eye_calibration_client::TriggerRequest& req,
88  rc_hand_eye_calibration_client::TriggerResponse& res);
89 
92  bool setSlotSrv(rc_hand_eye_calibration_client::SetCalibrationPoseRequest& req,
93  rc_hand_eye_calibration_client::SetCalibrationPoseResponse& res);
94 
96  bool setCalibSrv(rc_hand_eye_calibration_client::SetCalibrationRequest& req,
97  rc_hand_eye_calibration_client::SetCalibrationResponse& res);
98 
100  void advertiseServices();
101 
102  void initConfiguration();
103 
106  void initTimers();
107 
108  void dynamicReconfigureCb(rc_hand_eye_calibration_client::hand_eye_calibrationConfig& config, uint32_t);
109 
111 
113  void calibResponseHandler(CalibrationResponse& res);
114 
116  void updateCalibrationCache(const rc_hand_eye_calibration_client::CalibrationResponse& res);
117 
118  // ROS Stuff
132  geometry_msgs::TransformStamped current_calibration_; // initialized all-zero
134  std::string camera_frame_id_ = "camera";
136  std::string endeff_frame_id_ = "end_effector";
137  std::string base_frame_id_ = "base_link";
140  double calib_publish_period_ = 0.0; // seconds
144  double calib_request_period_ = 0.0; // seconds
146  std::unique_ptr<dynamic_reconfigure::Server<rc_hand_eye_calibration_client::hand_eye_calibrationConfig> > server_;
147 
148  // REST stuff
150 
151  std::tuple<size_t, size_t, size_t> image_version_;
152 };
153 
154 } // namespace rc_hand_eye_calibration_client
155 
156 #endif // RC_HAND_EYE_CALIBRATION_CLIENT_H
bool calibSrv(rc_hand_eye_calibration_client::CalibrationRequest &req, rc_hand_eye_calibration_client::CalibrationResponse &res)
Perform the calibration and return its result.
bool removeSrv(rc_hand_eye_calibration_client::TriggerRequest &req, rc_hand_eye_calibration_client::TriggerResponse &res)
Remove calibration so sensor reports as uncalibrated.
bool getCalibResultSrv(rc_hand_eye_calibration_client::CalibrationRequest &req, rc_hand_eye_calibration_client::CalibrationResponse &res)
Service call to get the result. Also returned directly via calibrate.
void advertiseServices()
Advertises the services in the namespace of nh_.
bool setCalibSrv(rc_hand_eye_calibration_client::SetCalibrationRequest &req, rc_hand_eye_calibration_client::SetCalibrationResponse &res)
Service call to set the calibration.
void sendCachedCalibration(const ros::SteadyTimerEvent &=ros::SteadyTimerEvent())
void updateCalibrationCache(const rc_hand_eye_calibration_client::CalibrationResponse &res)
Copy resp into current_calibration_.
std::string camera_frame_id_
Default Frame Ids to be used for the tf broadcast.
void calibResponseHandler(CalibrationResponse &res)
update and broadcast the internally cached calibration result.
std::unique_ptr< dynamic_reconfigure::Server< rc_hand_eye_calibration_client::hand_eye_calibrationConfig > > server_
tf2_ros::TransformBroadcaster dynamic_tf2_broadcaster_
To be used for periodic sending.
tf2_ros::StaticTransformBroadcaster static_tf2_broadcaster_
To be used for on-change sending only.
std::string endeff_frame_id_
Which one is used depends on the value of "robot_mounted" in the calibration response.
void initTimers()
Initialize timers to periodically publish and/or request the calibration Depends on parameters: calib...
void requestCalibration(const ros::SteadyTimerEvent &)
Wraps the above getCalibResultSrv.
bool callService(const std::string &name, const Request &req, Response &res)
double calib_publish_period_
Whether to send the transformation periodically instead of as a static transformation Zero or negativ...
bool setSlotSrv(rc_hand_eye_calibration_client::SetCalibrationPoseRequest &req, rc_hand_eye_calibration_client::SetCalibrationPoseResponse &res)
Save given pose and the pose of the grid indexed by the given request.slot.
bool saveSrv(rc_hand_eye_calibration_client::TriggerRequest &req, rc_hand_eye_calibration_client::TriggerResponse &res)
store hand eye calibration on sensor
double calib_request_period_
If non-negative: Periodically update the calibration by requesting it.
void dynamicReconfigureCb(rc_hand_eye_calibration_client::hand_eye_calibrationConfig &config, uint32_t)
bool resetSrv(rc_hand_eye_calibration_client::TriggerRequest &req, rc_hand_eye_calibration_client::TriggerResponse &res)
Delete all stored data, e.g., to start over.


rc_hand_eye_calibration_client
Author(s): Christian Emmerich
autogenerated on Sat Feb 13 2021 03:41:55