33 #ifndef RC_HAND_EYE_CALIBRATION_CLIENT_H 34 #define RC_HAND_EYE_CALIBRATION_CLIENT_H 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> 43 #include <dynamic_reconfigure/server.h> 47 #include <geometry_msgs/TransformStamped.h> 61 template <
typename Request,
typename Response>
62 bool callService(
const std::string& name,
const Request& req, Response& res);
67 bool calibSrv(rc_hand_eye_calibration_client::CalibrationRequest& req,
68 rc_hand_eye_calibration_client::CalibrationResponse& res);
72 rc_hand_eye_calibration_client::CalibrationResponse& res);
79 bool saveSrv(rc_hand_eye_calibration_client::TriggerRequest& req,
80 rc_hand_eye_calibration_client::TriggerResponse& res);
83 bool resetSrv(rc_hand_eye_calibration_client::TriggerRequest& req,
84 rc_hand_eye_calibration_client::TriggerResponse& res);
87 bool removeSrv(rc_hand_eye_calibration_client::TriggerRequest& req,
88 rc_hand_eye_calibration_client::TriggerResponse& res);
92 bool setSlotSrv(rc_hand_eye_calibration_client::SetCalibrationPoseRequest& req,
93 rc_hand_eye_calibration_client::SetCalibrationPoseResponse& res);
96 bool setCalibSrv(rc_hand_eye_calibration_client::SetCalibrationRequest& req,
97 rc_hand_eye_calibration_client::SetCalibrationResponse& res);
108 void dynamicReconfigureCb(rc_hand_eye_calibration_client::hand_eye_calibrationConfig& config, uint32_t);
146 std::unique_ptr<dynamic_reconfigure::Server<rc_hand_eye_calibration_client::hand_eye_calibrationConfig> >
server_;
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.
ros::SteadyTimer calib_request_timer_
ros::ServiceServer srv_get_result_
ros::ServiceServer srv_set_slot_
ros::SteadyTimer calib_publish_timer_
ros::ServiceServer srv_calibrate_
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.
ros::ServiceServer srv_remove_
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_.
ros::ServiceServer srv_save_
ros::ServiceServer srv_publish_transform_
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_
std::string base_frame_id_
tf2_ros::TransformBroadcaster dynamic_tf2_broadcaster_
To be used for periodic sending.
ros::ServiceServer srv_set_calibration_
ros::ServiceServer srv_reset_
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.
std::tuple< size_t, size_t, size_t > image_version_
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.
rc_rest_api::RestHelper rest_helper_
bool callService(const std::string &name, const Request &req, Response &res)
geometry_msgs::TransformStamped current_calibration_
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.
HandEyeCalibClient(std::string host, ros::NodeHandle nh)
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.