Skip to content

Commit

Permalink
[RobotInterface] change depth cameras to rgbd cameras
Browse files Browse the repository at this point in the history
  • Loading branch information
@prashanthr05
prashanthr05 committed Sep 13, 2020
1 parent c2dac20 commit fef8401
Show file tree
Hide file tree
Showing 5 changed files with 43 additions and 39 deletions.
6 changes: 3 additions & 3 deletions ...tInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,11 +150,11 @@ class YarpSensorBridge : public ISensorBridge,
bool getRGBCamerasList(std::vector<std::string>& rgbCamerasList) final;

/**
* Get depth cameras
* @param[out] depthCamerasList list of depth cameras attached to the bridge
* Get RGBD cameras
* @param[out] rgbdCamerasList list of depth cameras attached to the bridge
* @return true/false in case of success/failure
*/
bool getDepthCamerasList(std::vector<std::string>& depthCamerasList) final;
bool getRGBDCamerasList(std::vector<std::string>& rgbdCamerasList) final;

/**
* Get joint position in radians
Expand Down
55 changes: 29 additions & 26 deletions ...erface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,8 @@ struct YarpSensorBridge::Impl
std::unordered_map<std::string, Vector6d> wholeBodyFTMeasures; /** < map holding six axis force torque measures */
std::unordered_map<std::string, Eigen::Vector3d> wholeBodyInertialMeasures; /** < map holding three axis inertial sensor measures */
std::unordered_map<std::string, Vector6d> wholeBodyCartesianWrenchMeasures; /** < map holding cartesian wrench measures */
std::unordered_map<std::string, Eigen::MatrixXd> wholeBodyCameraImages; /** < map holding images **/
std::unordered_map<std::string, Eigen::MatrixXd> wholeBodyCameraRGBImages; /** < map holding images **/
std::unordered_map<std::string, Eigen::MatrixXd> wholeBodyCameraDepthImages; /** < map holding images **/

const int nrChannelsInYARPGenericIMUSensor{12};
const int nrChannelsInYARPGenericCartesianWrench{6};
Expand Down Expand Up @@ -282,41 +283,41 @@ struct YarpSensorBridge::Impl
{
std::pair<int, int> imgDimensions(rgbWidth[idx], rgbHeight[idx]);
auto cameraName{metaData.sensorsList.rgbCamerasList[idx]};
metaData.bridgeOptions.imgDimensions[cameraName] = imgDimensions;
metaData.bridgeOptions.rgbImgDimensions[cameraName] = imgDimensions;
}
}

if (ptr->getParameter("depth_cameras_list", metaData.sensorsList.depthCamerasList, VectorResizeMode::Resizable))
if (ptr->getParameter("rgbd_cameras_list", metaData.sensorsList.rgbdCamerasList, VectorResizeMode::Resizable))
{
metaData.bridgeOptions.isCameraEnabled = true;

std::vector<int> depthCamWidth, depthCamHeight;
if (ptr->getParameter("depth_image_width", depthCamWidth, VectorResizeMode::Resizable))
std::vector<int> rgbdCamWidth, rgbdCamHeight;
if (ptr->getParameter("rgbd_image_width", rgbdCamWidth, VectorResizeMode::Resizable))
{
std::cerr << logPrefix << " Required parameter \"depth_image_width\" not available in the configuration"
std::cerr << logPrefix << " Required parameter \"rgbd_image_width\" not available in the configuration"
<< std::endl;
return false;
}

if (ptr->getParameter("depth_image_height", depthCamHeight, VectorResizeMode::Resizable))
if (ptr->getParameter("rgbd_image_height", rgbdCamHeight, VectorResizeMode::Resizable))
{
std::cerr << logPrefix << " Required parameter \"depth_image_height\" not available in the configuration"
std::cerr << logPrefix << " Required parameter \"rgbd_image_height\" not available in the configuration"
<< std::endl;
return false;
}

if ( (depthCamWidth.size() != metaData.sensorsList.depthCamerasList.size()) ||
(depthCamHeight.size() != metaData.sensorsList.depthCamerasList.size()) )
if ( (rgbdCamWidth.size() != metaData.sensorsList.rgbdCamerasList.size()) ||
(rgbdCamHeight.size() != metaData.sensorsList.rgbdCamerasList.size()) )
{
std::cerr << logPrefix << " Parameters list size mismatch" << std::endl;
return false;
}

for (int idx = 0; idx < depthCamHeight.size(); idx++)
for (int idx = 0; idx < rgbdCamHeight.size(); idx++)
{
std::pair<int, int> imgDimensions(depthCamWidth[idx], depthCamHeight[idx]);
auto cameraName{metaData.sensorsList.depthCamerasList[idx]};
metaData.bridgeOptions.imgDimensions[cameraName] = imgDimensions;
std::pair<int, int> imgDimensions(rgbdCamWidth[idx], rgbdCamHeight[idx]);
auto cameraName{metaData.sensorsList.rgbdCamerasList[idx]};
metaData.bridgeOptions.rgbdImgDimensions[cameraName] = imgDimensions;
}
}

Expand Down Expand Up @@ -955,21 +956,29 @@ struct YarpSensorBridge::Impl
std::string_view interfaceType{"RGB Cameras"};
if (!attachAllCamerasOfSpecificType(devList,
metaData.sensorsList.rgbCamerasList,
metaData.bridgeOptions.imgDimensions,
metaData.bridgeOptions.rgbImgDimensions,
interfaceType,
wholeBodyFrameGrabberInterface,
wholeBodyCameraImages))
wholeBodyCameraRGBImages))
{
return false;
}

std::string_view interfaceTypeDepth = "Depth Cameras";
std::string_view interfaceTypeDepth = "RGBD Cameras";
if (!attachAllCamerasOfSpecificType(devList,
metaData.sensorsList.depthCamerasList,
metaData.bridgeOptions.imgDimensions,
metaData.sensorsList.rgbdCamerasList,
metaData.bridgeOptions.rgbdImgDimensions,
interfaceTypeDepth,
wholeBodyRGBDInterface,
wholeBodyCameraImages))
wholeBodyCameraDepthImages))
{
return false;
}

// resize also rgb images of RGBD cameras
if (!resizeImageBuffers(metaData.sensorsList.rgbdCamerasList,
metaData.bridgeOptions.rgbdImgDimensions,
wholeBodyCameraRGBImages))
{
return false;
}
Expand Down Expand Up @@ -997,12 +1006,6 @@ struct YarpSensorBridge::Impl
}
}

if (sensorMap.size() != camList.size())
{
std::cout << logPrefix << " could not attach all desired rgb cameras " << interfaceType << "." << std::endl;
return false;
}

if (!resizeImageBuffers(camList,
imgDimensionsMap,
imgBuffersMap))
Expand Down
6 changes: 3 additions & 3 deletions src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,13 +176,13 @@ bool YarpSensorBridge::getRGBCamerasList(std::vector<std::string>& rgbCamerasLis
return true;
}

bool YarpSensorBridge::getDepthCamerasList(std::vector<std::string>& depthCamerasList)
bool YarpSensorBridge::getRGBDCamerasList(std::vector<std::string>& rgbdCamerasList)
{
if (!m_pimpl->checkValid("[YarpSensorBridge::getDepthCamerasList]"))
if (!m_pimpl->checkValid("[YarpSensorBridge::getRGBDCamerasList]"))
{
return false;
}
depthCamerasList = m_pimpl->metaData.sensorsList.depthCamerasList;
rgbdCamerasList = m_pimpl->metaData.sensorsList.rgbdCamerasList;
return true;
}

Expand Down
11 changes: 6 additions & 5 deletions src/RobotInterface/include/BipedalLocomotion/RobotInterface/ISensorBridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@ struct SensorBridgeOptions
bool isCameraEnabled{false}; /**< flag to connect camera sources */

size_t nrJoints{0}; /**< number of joints available through Kinematics stream, to be configured at initialization */
std::unordered_map<std::string, std::pair<int, int> > imgDimensions; /**< dimensions of the camera images available through camera streams, to be configured at initialization */
std::unordered_map<std::string, std::pair<int, int> > rgbImgDimensions; /**< dimensions of the images available through rgb camera streams, to be configured at initialization */
std::unordered_map<std::string, std::pair<int, int> > rgbdImgDimensions; /**< dimensions of the depth images available through rgbd camera streams, to be configured at initialization */
};

/**
Expand All @@ -61,7 +62,7 @@ struct SensorLists
std::vector<std::string> threeAxisForceTorqueSensorsList; /**< list of three axis force torque sensors attached to the bridge */
std::vector<std::string> cartesianWrenchesList; /**< list of cartesian wrench streams attached to the bridge */
std::vector<std::string> rgbCamerasList; /**< list of rgb cameras attached to the bridge */
std::vector<std::string> depthCamerasList; /**< list of depth cameras attached to the bridge */
std::vector<std::string> rgbdCamerasList; /**< list of RGBD cameras attached to the bridge */
};


Expand Down Expand Up @@ -164,11 +165,11 @@ class ISensorBridge
virtual bool getRGBCamerasList(std::vector<std::string>& rgbCamerasList) = 0;

/**
* Get depth cameras
* @param[out] depthCamerasList list of depth cameras attached to the bridge
* Get RGBD cameras
* @param[out] rgbdCamerasList list of rgbd cameras attached to the bridge
* @return true/false in case of success/failure
*/
virtual bool getDepthCamerasList(std::vector<std::string>& depthCamerasList) = 0;
virtual bool getRGBDCamerasList(std::vector<std::string>& rgbdCamerasList) = 0;

/**
* Get joint position in radians
Expand Down
4 changes: 2 additions & 2 deletions src/RobotInterface/tests/DummyImplementation/DummySensorBridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,9 +97,9 @@ class DummySensorBridge : public ISensorBridge
rgbCamerasList = std::vector<std::string>{""};
return true;
};
virtual bool getDepthCamerasList(std::vector<std::string>& depthCamerasList) override
virtual bool getRGBDCamerasList(std::vector<std::string>& rgbdCamerasList) override
{
depthCamerasList = std::vector<std::string>{""};
rgbdCamerasList = std::vector<std::string>{""};
return true;
};

Expand Down

0 comments on commit fef8401

Please sign in to comment.