Drake
depth_sensor.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 #include <vector>
6 
18 
19 namespace drake {
20 namespace systems {
21 namespace sensors {
22 
97 
98 class DepthSensor : public systems::LeafSystem<double> {
99  public:
101 
102 
103  DepthSensor(const std::string& name, const RigidBodyTree<double>& tree,
117  const RigidBodyFrame<double>& frame,
118  const DepthSensorSpecification& specification);
119 
121  const RigidBodyTree<double>& get_tree() const { return tree_; }
122 
125  const RigidBodyFrame<double>& get_frame() const { return frame_; }
126 
128  const DepthSensorSpecification& get_specification() { return specification_; }
129 
133  int get_num_pitch() const { return specification_.num_pitch_values(); }
134 
138  int get_num_yaw() const { return specification_.num_yaw_values(); }
139 
143  return specification_.num_depth_readings();
144  }
145 
149  const;
150 
154 
158 
159  friend std::ostream& operator<<(std::ostream& out,
160  const DepthSensor& depth_sensor);
161 
162  private:
163  // These are calculators for the depth data and sensor pose outputs.
164  void CalcDepthOutput(const Context<double>& context,
165  DepthSensorOutput<double>* data_output) const;
166 
167  void CalcPoseOutput(const Context<double>& context,
168  rendering::PoseVector<double>* pose_output) const;
169 
170 
171  // The depth sensor will cast a ray with its start point at (0,0,0) in the
172  // sensor's base frame (as defined by get_frame()). Its end, also in the
173  // sensor's base frame, is computed by this method and stored in
174  // raycast_endpoints_ at the time of construction. raycast_endpoints_ is only
175  // computed once at the time of construction since the end points are constant
176  // in the sensor's base frame.
177  void PrecomputeRaycastEndpoints();
178 
179  // Applies the min / max range of the sensor. Any measurement that is less
180  // than the minimum or greater than the maximum is set to an invalid value.
181  // This is so users of this sensor can distinguish between an object at the
182  // maximum sensing distance and not detecting any object within the sensing
183  // range.
184  void ApplyLimits(VectorX<double>* dists) const;
185 
186  const std::string name_;
187  const RigidBodyTree<double>& tree_;
188  const RigidBodyFrame<double> frame_;
189  const DepthSensorSpecification specification_;
190  int input_port_index_{};
191  int depth_output_port_index_{};
192  int pose_output_port_index_{};
193 
194  // A cache of where a depth measurement ray endpoint would be if the maximum
195  // range were achieved. This is cached to avoid repeated allocation and
196  // computation.
197  Eigen::Matrix3Xd raycast_endpoints_;
198 };
199 
200 } // namespace sensors
201 } // namespace systems
202 } // namespace drake
int get_num_pitch() const
Returns the number of pixel rows in the resulting depth sensor output.
Definition: depth_sensor.h:133
const InputPortDescriptor< double > & get_rigid_body_tree_state_input_port() const
Returns a descriptor of the input port containing the generalized state of the RigidBodyTree.
Definition: depth_sensor.cc:120
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
const DepthSensorSpecification & get_specification()
Returns this sensor&#39;s specification.
Definition: depth_sensor.h:128
Definition: automotive_demo.cc:88
int num_yaw_values() const
Definition: depth_sensor_specification.h:66
friend std::ostream & operator<<(std::ostream &out, const DepthSensor &depth_sensor)
Definition: depth_sensor.cc:207
const OutputPort< double > & get_sensor_state_output_port() const
Returns the state output port, which contains the sensor&#39;s sensed values.
Definition: depth_sensor.cc:124
DepthSensor(const DepthSensor &)=delete
int num_depth_readings() const
Definition: depth_sensor_specification.cc:41
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
const RigidBodyFrame< double > & get_frame() const
Returns this sensor&#39;s frame, which specifies its location and orientation in the RigidBodyTree.
Definition: depth_sensor.h:125
int get_num_yaw() const
Returns the number of pixel columns in the resulting depth sensor output.
Definition: depth_sensor.h:138
const OutputPort< double > & get_pose_output_port() const
Returns the X_WS output port, which contains the transform from this sensor&#39;s frame to the world fram...
Definition: depth_sensor.cc:128
Holds a DepthSensor&#39;s specification.
Definition: depth_sensor_specification.h:16
const RigidBodyTree< double > & get_tree() const
Returns the RigidBodyTree that this sensor is sensing.
Definition: depth_sensor.h:121
int num_pitch_values() const
Definition: depth_sensor_specification.h:67
A simple model of an ideal depth sensor.
Definition: depth_sensor.h:98
Specializes BasicVector with specific getters and setters that are useful for consumers of DepthSenso...
Definition: depth_sensor_output.h:18
int get_num_depth_readings() const
Returns the size of the system&#39;s output, which equals the product of num_pitch() and num_yaw()...
Definition: depth_sensor.h:142
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:35
Provides careful macros to selectively enable or disable the special member functions for copy-constr...