Program Listing for File depth_camera_info.hpp
↰ Return to documentation for file (/tmp/ws/src/robot_calibration/robot_calibration/include/robot_calibration/util/depth_camera_info.hpp
)
/*
* Copyright (C) 2022 Michael Ferguson
* Copyright (C) 2015-2016 Fetch Robotics Inc.
* Copyright (C) 2013-2014 Unbounded Robotics Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
// Author: Michael Ferguson
#ifndef ROBOT_CALIBRATION_CAPTURE_DEPTH_CAMERA_H
#define ROBOT_CALIBRATION_CAPTURE_DEPTH_CAMERA_H
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/parameter_client.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <robot_calibration_msgs/msg/calibration_data.hpp>
#include <robot_calibration_msgs/msg/extended_camera_info.hpp>
namespace robot_calibration
{
class DepthCameraInfoManager
{
public:
DepthCameraInfoManager() : camera_info_valid_(false) {}
virtual ~DepthCameraInfoManager() {}
bool init(const std::string& name, rclcpp::Node::SharedPtr node, const rclcpp::Logger& logger)
{
std::string topic_name =
node->declare_parameter<std::string>(name + ".camera_info_topic", "/head_camera/depth/camera_info");
camera_info_subscriber_ = node->create_subscription<sensor_msgs::msg::CameraInfo>(
topic_name, 1, std::bind(&DepthCameraInfoManager::cameraInfoCallback, this, std::placeholders::_1));
// Set default driver parameters
z_offset_mm_ = 0;
z_scaling_ = 1.0;
// Get parameters of drivers
std::string driver_name =
node->declare_parameter<std::string>(name + ".camera_driver", "/head_camera/driver");
auto params_client = std::make_shared<rclcpp::SyncParametersClient>(node, driver_name);
if (params_client->wait_for_service(std::chrono::seconds(10)))
{
auto parameters = params_client->get_parameters({"z_offset_mm", "z_scaling"});
for (auto& param : parameters)
{
if (param.get_name() == "z_offset_mm")
{
z_offset_mm_ = param.as_int();
RCLCPP_INFO(node->get_logger(), "Got value of %f for z_offset_mm", z_offset_mm_);
}
else if (param.get_name() == "z_scaling")
{
z_scaling_ = param.as_double();
RCLCPP_INFO(node->get_logger(), "Got value of %f for z_scaling", z_scaling_);
}
}
}
else
{
RCLCPP_WARN(node->get_logger(), "Unable to get parameters from %s", driver_name.c_str());
}
// Wait for camera_info
int count = 25;
while (--count)
{
if (camera_info_valid_)
{
return true;
}
rclcpp::sleep_for(std::chrono::milliseconds(100));
rclcpp::spin_some(node);
}
RCLCPP_WARN(logger, "CameraInfo receive timed out.");
return false;
}
robot_calibration_msgs::msg::ExtendedCameraInfo getDepthCameraInfo()
{
robot_calibration_msgs::msg::ExtendedCameraInfo info;
info.camera_info = *camera_info_ptr_;
info.parameters.resize(2);
info.parameters[0].name = "z_offset_mm";
info.parameters[0].value = z_offset_mm_;
info.parameters[1].name = "z_scaling";
info.parameters[1].value = z_scaling_;
return info;
}
private:
void cameraInfoCallback(sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info)
{
camera_info_ptr_ = camera_info;
camera_info_valid_ = true;
}
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_subscriber_;
bool camera_info_valid_;
sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_ptr_;
double z_offset_mm_;
double z_scaling_;
};
} // namespace robot_calibration
#endif // ROBOT_CALIBRATION_CAPTURE_DEPTH_CAMERA_H