stereo_camera.h
Go to the documentation of this file.
1 #pragma once
2 
4 
5 #include <ensenso_camera_msgs/RequestDataAction.h>
6 #include <ensenso_camera_msgs/LocatePatternAction.h>
7 #include <ensenso_camera_msgs/ProjectPatternAction.h>
8 #include <ensenso_camera_msgs/TexturedPointCloudAction.h>
9 #include <ensenso_camera_msgs/TelecentricProjectionAction.h>
10 
19 
25 {
26  projectorDontCare, //@< Inherit the projector settings from the parameter set.
27  projectorOn, //@< Enable the projector and disable the front light.
28  projectorOff //@< Enable the front light and disable the projector.
29 };
30 
31 class StereoCamera : public Camera
32 {
33 private:
34  std::string robotFrame;
35  std::string wristFrame;
36 
37  sensor_msgs::CameraInfoPtr leftCameraInfo;
38  sensor_msgs::CameraInfoPtr rightCameraInfo;
39  sensor_msgs::CameraInfoPtr leftRectifiedCameraInfo;
40  sensor_msgs::CameraInfoPtr rightRectifiedCameraInfo;
41 
42  std::unique_ptr<RequestDataServer> requestDataServer;
43  std::unique_ptr<CalibrateHandEyeServer> calibrateHandEyeServer;
44  std::unique_ptr<CalibrateWorkspaceServer> calibrateWorkspaceServer;
45  std::unique_ptr<FitPrimitiveServer> fitPrimitiveServer;
46  std::unique_ptr<LocatePatternServer> locatePatternServer;
47  std::unique_ptr<ProjectPatternServer> projectPatternServer;
48  std::unique_ptr<TexturedPointCloudServer> texturedPointCloudServer;
49  std::unique_ptr<TelecentricProjectionServer> telecentricProjectionServer;
50 
58 
60 
61  // Information that we remember between the different steps of the hand eye
62  // calibration.
63  // We save the pattern buffer outside of the NxLib, because otherwise we
64  // could not use the LocatePattern action while a hand eye calibration is
65  // in progress.
67  std::vector<tf2::Transform> handEyeCalibrationRobotPoses;
68 
69 public:
70  StereoCamera(ros::NodeHandle nh, std::string serial, std::string fileCameraPath, bool fixed, std::string cameraFrame,
71  std::string targetFrame, std::string robotFrame, std::string wristFrame, std::string linkFrame);
72 
73  bool open() override;
74 
79  void startServers() const override;
80 
84  void onSetParameter(ensenso_camera_msgs::SetParameterGoalConstPtr const& goal) override;
88  void onRequestData(ensenso_camera_msgs::RequestDataGoalConstPtr const& goal);
92  void onLocatePattern(ensenso_camera_msgs::LocatePatternGoalConstPtr const& goal);
96  void onProjectPattern(ensenso_camera_msgs::ProjectPatternGoalConstPtr const& goal);
100  void onCalibrateHandEye(ensenso_camera_msgs::CalibrateHandEyeGoalConstPtr const& goal);
104  void onCalibrateWorkspace(ensenso_camera_msgs::CalibrateWorkspaceGoalConstPtr const& goal);
108  void onFitPrimitive(ensenso_camera_msgs::FitPrimitiveGoalConstPtr const& goal);
109 
113  void onTexturedPointCloud(ensenso_camera_msgs::TexturedPointCloudGoalConstPtr const& goal);
114 
118  void onTelecentricProjection(ensenso_camera_msgs::TelecentricProjectionGoalConstPtr const& goal);
119 private:
127  void saveParameterSet(std::string name, bool projectorWasSet);
128 
137  void loadParameterSet(std::string name, ProjectorState projector = projectorDontCare);
138 
142  ros::Time capture() const override;
143 
148  std::vector<StereoCalibrationPattern> collectPattern(bool clearBuffer = false) const;
149 
159  void fillCameraInfoFromNxLib(sensor_msgs::CameraInfoPtr const& info, bool right, bool rectified = false) const;
164  void updateCameraInfo() override;
165 
166  ensenso_camera_msgs::ParameterPtr readParameter(std::string const& key) const override;
167  void writeParameter(ensenso_camera_msgs::Parameter const& parameter) override;
168 };
std::vector< tf2::Transform > handEyeCalibrationRobotPoses
Definition: stereo_camera.h:67
std::unique_ptr< FitPrimitiveServer > fitPrimitiveServer
Definition: stereo_camera.h:45
void writeParameter(ensenso_camera_msgs::Parameter const &parameter) override
void onFitPrimitive(ensenso_camera_msgs::FitPrimitiveGoalConstPtr const &goal)
void onProjectPattern(ensenso_camera_msgs::ProjectPatternGoalConstPtr const &goal)
bool fixed
Definition: camera.h:194
std::string targetFrame
Definition: camera.h:198
image_transport::CameraPublisher leftRectifiedImagePublisher
Definition: stereo_camera.h:53
ros::NodeHandle nh
Definition: camera.h:200
void onCalibrateWorkspace(ensenso_camera_msgs::CalibrateWorkspaceGoalConstPtr const &goal)
void onTexturedPointCloud(ensenso_camera_msgs::TexturedPointCloudGoalConstPtr const &goal)
image_transport::Publisher disparityMapPublisher
Definition: stereo_camera.h:55
std::unique_ptr< ProjectPatternServer > projectPatternServer
Definition: stereo_camera.h:47
std::string cameraFrame
Definition: camera.h:196
image_transport::Publisher projectedImagePublisher
Definition: stereo_camera.h:57
ProjectorState
Definition: stereo_camera.h:24
std::string handEyeCalibrationPatternBuffer
Definition: stereo_camera.h:66
std::unique_ptr< CalibrateWorkspaceServer > calibrateWorkspaceServer
Definition: stereo_camera.h:44
sensor_msgs::CameraInfoPtr leftCameraInfo
Definition: stereo_camera.h:37
ros::Publisher projectedPointCloudPublisher
Definition: stereo_camera.h:59
ros::Publisher pointCloudPublisherColor
Definition: stereo_camera.h:59
image_transport::CameraPublisher depthImagePublisher
Definition: stereo_camera.h:56
std::string serial
Definition: camera.h:187
std::unique_ptr< CalibrateHandEyeServer > calibrateHandEyeServer
Definition: stereo_camera.h:43
std::vector< StereoCalibrationPattern > collectPattern(bool clearBuffer=false) const
void startServers() const override
std::unique_ptr< LocatePatternServer > locatePatternServer
Definition: stereo_camera.h:46
sensor_msgs::CameraInfoPtr leftRectifiedCameraInfo
Definition: stereo_camera.h:39
void loadParameterSet(std::string name, ProjectorState projector=projectorDontCare)
ros::Time capture() const override
std::string fileCameraPath
Definition: camera.h:182
ros::Publisher pointCloudPublisher
Definition: stereo_camera.h:59
void onSetParameter(ensenso_camera_msgs::SetParameterGoalConstPtr const &goal) override
std::unique_ptr< RequestDataServer > requestDataServer
Definition: stereo_camera.h:42
std::unique_ptr< TexturedPointCloudServer > texturedPointCloudServer
Definition: stereo_camera.h:48
void fillCameraInfoFromNxLib(sensor_msgs::CameraInfoPtr const &info, bool right, bool rectified=false) const
sensor_msgs::CameraInfoPtr rightRectifiedCameraInfo
Definition: stereo_camera.h:40
std::string linkFrame
Definition: camera.h:197
void saveParameterSet(std::string name, bool projectorWasSet)
std::string robotFrame
Definition: stereo_camera.h:34
void onRequestData(ensenso_camera_msgs::RequestDataGoalConstPtr const &goal)
void onCalibrateHandEye(ensenso_camera_msgs::CalibrateHandEyeGoalConstPtr const &goal)
StereoCamera(ros::NodeHandle nh, std::string serial, std::string fileCameraPath, bool fixed, std::string cameraFrame, std::string targetFrame, std::string robotFrame, std::string wristFrame, std::string linkFrame)
void onLocatePattern(ensenso_camera_msgs::LocatePatternGoalConstPtr const &goal)
image_transport::CameraPublisher leftRawImagePublisher
Definition: stereo_camera.h:51
void updateCameraInfo() override
image_transport::CameraPublisher rightRawImagePublisher
Definition: stereo_camera.h:52
bool open() override
ensenso_camera_msgs::ParameterPtr readParameter(std::string const &key) const override
image_transport::CameraPublisher rightRectifiedImagePublisher
Definition: stereo_camera.h:54
Definition: camera.h:178
sensor_msgs::CameraInfoPtr rightCameraInfo
Definition: stereo_camera.h:38
void onTelecentricProjection(ensenso_camera_msgs::TelecentricProjectionGoalConstPtr const &goal)
std::unique_ptr< TelecentricProjectionServer > telecentricProjectionServer
Definition: stereo_camera.h:49
std::string wristFrame
Definition: stereo_camera.h:35


ensenso_camera
Author(s): Ensenso
autogenerated on Sat Jul 27 2019 03:51:24