$search
GazeboRosStereoCamera Controller. More...
#include <gazebo_ros_stereo_camera.h>
Public Member Functions | |
GazeboRosStereoCamera (Entity *parent) | |
Constructor. | |
const float * | GetDepthData (unsigned int i=0) |
Get a point to the depth data. | |
virtual | ~GazeboRosStereoCamera () |
Destructor. | |
Protected Member Functions | |
virtual void | FiniChild () |
Finalize the controller, unadvertise topics. | |
virtual void | InitChild () |
Init the controller. | |
virtual void | LoadChild (XMLConfigNode *node) |
Load the controller. | |
virtual void | UpdateChild () |
Update the controller. | |
Private Member Functions | |
bool | fillDepthImage (sensor_msgs::PointCloud &point_cloud, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg) |
helper function fill depth image | |
void | ImageConnect () |
void | ImageDisconnect () |
void | InfoConnect () |
void | InfoDisconnect () |
void | PointCloudConnect () |
void | PointCloudDisconnect () |
void | PublishCameraInfo () |
Publish CameraInfo to the ROS topic. | |
void | PutCameraData () |
Put camera data to the ROS topic. | |
Private Attributes | |
ros::Publisher | camera_info_pub_ |
sensor_msgs::CameraInfo | cameraInfoMsg |
std::string | cameraInfoTopicName |
ROS camera_info topic name. | |
ParamT< std::string > * | cameraInfoTopicNameP |
double | Cx |
ParamT< double > * | CxP |
double | CxPrime |
ParamT< double > * | CxPrimeP |
double | Cy |
ParamT< double > * | CyP |
int | depth |
double | distortion_k1 |
ParamT< double > * | distortion_k1P |
double | distortion_k2 |
ParamT< double > * | distortion_k2P |
double | distortion_k3 |
ParamT< double > * | distortion_k3P |
double | distortion_t1 |
ParamT< double > * | distortion_t1P |
double | distortion_t2 |
ParamT< double > * | distortion_t2P |
double | focal_length |
ParamT< double > * | focal_lengthP |
std::string | frameName |
ROS frame transform name to use in the image message header. This should typically match the link name the sensor is attached. | |
ParamT< std::string > * | frameNameP |
double | hack_baseline |
ParamT< double > * | hack_baselineP |
int | height |
size of image buffer | |
ros::Publisher | image_pub_ |
int | imageConnectCount |
Keep track of number of connctions. | |
sensor_msgs::Image | imageMsg |
ROS image message. | |
std::string | imageTopicName |
ROS image topic name. | |
ParamT< std::string > * | imageTopicNameP |
Parameters. | |
int | infoConnectCount |
Keep track of number of connctions for CameraInfo. | |
boost::mutex | lock |
A mutex to lock access to fields that are used in ROS message callbacks. | |
StereoCameraSensor * | myParent |
A pointer to the parent camera sensor. | |
ros::Publisher | point_cloud_pub_ |
int | pointCloudConnectCount |
Keep track of number of connctions for point clouds. | |
sensor_msgs::PointCloud | pointCloudMsg |
ros message | |
std::string | pointCloudTopicName |
ROS pointCloud topic name. | |
ParamT< std::string > * | pointCloudTopicNameP |
std::string | robotNamespace |
ParamT< std::string > * | robotNamespaceP |
for setting ROS name space | |
ros::NodeHandle * | rosnode_ |
A pointer to the ROS node. A node will be instantiated if it does not exist. | |
int | skip |
std::string | type |
int | width |
GazeboRosStereoCamera Controller.
<model:physical name="camera_model"> <body:empty name="camera_body_name"> <sensor:camera name="camera_sensor"> <controller:gazebo_ros_camera name="controller-name" plugin="libgazebo_ros_camera.so"> <alwaysOn>true</alwaysOn> <updateRate>15.0</updateRate> <imageTopicName>camera_name/image_raw</imageTopicName> <pointCloudTopicName>camera_name/points</pointCloudTopicName> <cameraInfoTopicName>camera_name/camera_info</cameraInfoTopicName> <frameName>camera_body_name</frameName> </controller:gazebo_ros_camera> </sensor:camera> </body:empty> </model:phyiscal>
Definition at line 113 of file gazebo_ros_stereo_camera.h.
gazebo::GazeboRosStereoCamera::GazeboRosStereoCamera | ( | Entity * | parent | ) |
Constructor.
parent | The parent entity, must be a Model or a Sensor |
Definition at line 62 of file gazebo_ros_stereo_camera.cpp.
gazebo::GazeboRosStereoCamera::~GazeboRosStereoCamera | ( | ) | [virtual] |
Destructor.
Definition at line 100 of file gazebo_ros_stereo_camera.cpp.
bool gazebo::GazeboRosStereoCamera::fillDepthImage | ( | sensor_msgs::PointCloud & | point_cloud, | |
uint32_t | rows_arg, | |||
uint32_t | cols_arg, | |||
uint32_t | step_arg, | |||
void * | data_arg | |||
) | [private] |
helper function fill depth image
RORRY'S MODIFICATION baisically just goes through the depth data and scales the [0-1] to [0-255]
Definition at line 450 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::FiniChild | ( | ) | [protected, virtual] |
Finalize the controller, unadvertise topics.
Definition at line 340 of file gazebo_ros_stereo_camera.cpp.
const float* gazebo::GazeboRosStereoCamera::GetDepthData | ( | unsigned int | i = 0 |
) |
Get a point to the depth data.
void gazebo::GazeboRosStereoCamera::ImageConnect | ( | ) | [private] |
Definition at line 229 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::ImageDisconnect | ( | ) | [private] |
Definition at line 235 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::InfoConnect | ( | ) | [private] |
Definition at line 200 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::InfoDisconnect | ( | ) | [private] |
Definition at line 206 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::InitChild | ( | ) | [protected, virtual] |
Init the controller.
Compute camera parameters if set to 0
Definition at line 245 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::LoadChild | ( | XMLConfigNode * | node | ) | [protected, virtual] |
Load the controller.
node | XML config node |
Definition at line 125 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::PointCloudConnect | ( | ) | [private] |
Definition at line 213 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::PointCloudDisconnect | ( | ) | [private] |
Definition at line 219 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::PublishCameraInfo | ( | ) | [private] |
Publish CameraInfo to the ROS topic.
Definition at line 501 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::PutCameraData | ( | ) | [private] |
Put camera data to the ROS topic.
RORRY'S MODIFICATION
pointer to the float data
copy from depth to pointCloudMsg
Definition at line 358 of file gazebo_ros_stereo_camera.cpp.
void gazebo::GazeboRosStereoCamera::UpdateChild | ( | ) | [protected, virtual] |
Update the controller.
publish CameraInfo
Definition at line 317 of file gazebo_ros_stereo_camera.cpp.
Definition at line 169 of file gazebo_ros_stereo_camera.h.
Definition at line 174 of file gazebo_ros_stereo_camera.h.
std::string gazebo::GazeboRosStereoCamera::cameraInfoTopicName [private] |
ROS camera_info topic name.
Definition at line 203 of file gazebo_ros_stereo_camera.h.
ParamT<std::string>* gazebo::GazeboRosStereoCamera::cameraInfoTopicNameP [private] |
Definition at line 179 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::Cx [private] |
Definition at line 216 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::CxP [private] |
Definition at line 184 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::CxPrime [private] |
Definition at line 215 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::CxPrimeP [private] |
Definition at line 183 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::Cy [private] |
Definition at line 217 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::CyP [private] |
Definition at line 185 of file gazebo_ros_stereo_camera.h.
int gazebo::GazeboRosStereoCamera::depth [private] |
Definition at line 230 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::distortion_k1 [private] |
Definition at line 220 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::distortion_k1P [private] |
Definition at line 188 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::distortion_k2 [private] |
Definition at line 221 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::distortion_k2P [private] |
Definition at line 189 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::distortion_k3 [private] |
Definition at line 222 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::distortion_k3P [private] |
Definition at line 190 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::distortion_t1 [private] |
Definition at line 223 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::distortion_t1P [private] |
Definition at line 191 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::distortion_t2 [private] |
Definition at line 224 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::distortion_t2P [private] |
Definition at line 192 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::focal_length [private] |
Definition at line 218 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::focal_lengthP [private] |
Definition at line 186 of file gazebo_ros_stereo_camera.h.
std::string gazebo::GazeboRosStereoCamera::frameName [private] |
ROS frame transform name to use in the image message header. This should typically match the link name the sensor is attached.
Definition at line 206 of file gazebo_ros_stereo_camera.h.
ParamT<std::string>* gazebo::GazeboRosStereoCamera::frameNameP [private] |
Definition at line 180 of file gazebo_ros_stereo_camera.h.
double gazebo::GazeboRosStereoCamera::hack_baseline [private] |
Definition at line 219 of file gazebo_ros_stereo_camera.h.
ParamT<double>* gazebo::GazeboRosStereoCamera::hack_baselineP [private] |
Definition at line 187 of file gazebo_ros_stereo_camera.h.
int gazebo::GazeboRosStereoCamera::height [private] |
size of image buffer
Definition at line 230 of file gazebo_ros_stereo_camera.h.
Definition at line 169 of file gazebo_ros_stereo_camera.h.
int gazebo::GazeboRosStereoCamera::imageConnectCount [private] |
Keep track of number of connctions.
Definition at line 144 of file gazebo_ros_stereo_camera.h.
ROS image message.
Definition at line 173 of file gazebo_ros_stereo_camera.h.
std::string gazebo::GazeboRosStereoCamera::imageTopicName [private] |
ROS image topic name.
Definition at line 199 of file gazebo_ros_stereo_camera.h.
ParamT<std::string>* gazebo::GazeboRosStereoCamera::imageTopicNameP [private] |
Parameters.
Definition at line 177 of file gazebo_ros_stereo_camera.h.
int gazebo::GazeboRosStereoCamera::infoConnectCount [private] |
Keep track of number of connctions for CameraInfo.
Definition at line 154 of file gazebo_ros_stereo_camera.h.
boost::mutex gazebo::GazeboRosStereoCamera::lock [private] |
A mutex to lock access to fields that are used in ROS message callbacks.
Definition at line 227 of file gazebo_ros_stereo_camera.h.
StereoCameraSensor* gazebo::GazeboRosStereoCamera::myParent [private] |
A pointer to the parent camera sensor.
RORRY'S MODIFICATION / myParent is now a StereoCameraSensor
Definition at line 165 of file gazebo_ros_stereo_camera.h.
Definition at line 169 of file gazebo_ros_stereo_camera.h.
int gazebo::GazeboRosStereoCamera::pointCloudConnectCount [private] |
Keep track of number of connctions for point clouds.
Definition at line 149 of file gazebo_ros_stereo_camera.h.
ros message
Definition at line 235 of file gazebo_ros_stereo_camera.h.
std::string gazebo::GazeboRosStereoCamera::pointCloudTopicName [private] |
ROS pointCloud topic name.
Definition at line 201 of file gazebo_ros_stereo_camera.h.
ParamT<std::string>* gazebo::GazeboRosStereoCamera::pointCloudTopicNameP [private] |
Definition at line 178 of file gazebo_ros_stereo_camera.h.
std::string gazebo::GazeboRosStereoCamera::robotNamespace [private] |
Definition at line 196 of file gazebo_ros_stereo_camera.h.
ParamT<std::string>* gazebo::GazeboRosStereoCamera::robotNamespaceP [private] |
for setting ROS name space
Definition at line 195 of file gazebo_ros_stereo_camera.h.
A pointer to the ROS node. A node will be instantiated if it does not exist.
Definition at line 168 of file gazebo_ros_stereo_camera.h.
int gazebo::GazeboRosStereoCamera::skip [private] |
Definition at line 232 of file gazebo_ros_stereo_camera.h.
std::string gazebo::GazeboRosStereoCamera::type [private] |
Definition at line 231 of file gazebo_ros_stereo_camera.h.
int gazebo::GazeboRosStereoCamera::width [private] |
Definition at line 230 of file gazebo_ros_stereo_camera.h.