Forward controller compiled

src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_controllers)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(rclcpp REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(robotiq_2f_msgs REQUIRED)
+find_package(realtime_tools REQUIRED)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME}
+  src/robotiq_forward_controller.cpp
+)
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  controller_interface
+  hardware_interface
+  pluginlib
+  robotiq_2f_msgs
+  realtime_tools
+)
+pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml)
+
+
+# Install targets
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Install header files
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+# Install plugin description file
+install(FILES
+  controller_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_package()

src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml

@@ -0,0 +1,7 @@
+<library path="robotiq_2f_controllers">
+    <class name="robotiq_2f_controllers/RobotiqForwardController" type="robotiq_2f_controllers::RobotiqForwardController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Forward controller for Robotiq 2F Gripper.
+        </description>
+    </class>
+</library>

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp

@@ -0,0 +1,99 @@
+
+
+#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP
+#define ROBOTIQ_FORWARD_CONTROLLER_HPP
+
+#include <stddef.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include "rclcpp/qos.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include "realtime_tools/realtime_buffer.h"
+
+#include "robotiq_2f_msgs/msg/forward_command.hpp"
+
+namespace robotiq_2f_controllers
+{
+class RobotiqForwardController : public controller_interface::ControllerInterface
+{
+public:
+    CONTROLLER_INTERFACE_PUBLIC
+    RobotiqForwardController();
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_init() override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+    std::string joint_name_;
+    std::vector<std::string> command_interface_types_;
+
+    // communication interface
+    bool new_msg_ = false;
+    rclcpp::Subscription<robotiq_2f_msgs::msg::ForwardCommand>::SharedPtr command_subscriber_;
+    std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> command_msg_;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand>>
+    msg_external_ptr_;
+
+    // Command Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_effort_command_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    command_interface_map_ = {
+      {"position", &joint_position_command_interface_},
+      {"max_velocity", &max_velocity_command_interface_},
+      {"max_effort", &max_effort_command_interface_}};
+
+    double desired_position_;
+    double desired_max_velocity_;
+    double desired_max_effort_;
+
+};
+} // end namespace robotiq_2f_controllers
+#endif

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller_old.hpp

@@ -0,0 +1,47 @@
+
+#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP
+#define ROBOTIQ_FORWARD_CONTROLLER_HPP
+
+#include <vector>
+#include <string>
+
+#include "controller_interface/controller_interface.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include "robotiq_2f_msgs/msg/forward_command.hpp"
+
+
+namespace robotiq_2f_controllers
+{
+class RobotiqForwardController : public controller_interface::ControllerInterface
+{
+public:
+    RobotiqForwardController();
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+    controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+private:
+    std::string joint_name_;
+    hardware_interface::LoanedCommandInterface position_command_;
+    hardware_interface::LoanedCommandInterface max_effort_command_;
+    hardware_interface::LoanedCommandInterface max_velocity_command_;
+
+    rclcpp::Subscription<robotiq_2f_msgs::msg::ForwardCommand>::SharedPtr command_subscriber_;
+    double desired_position_;
+    double desired_max_velocity_;
+    double desired_max_effort_;
+
+    void command_callback(const robotiq_2f_msgs::msg::ForwardCommand::SharedPtr msg);
+
+};
+}
+#endif

src/robotiq_2f/robotiq_2f_controllers/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robotiq_2f_controllers</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>robotiq_2f_msgs</depend>
+  <depend>realtime_tools</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp

@@ -0,0 +1,103 @@
+#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"
+
+
+using config_type = controller_interface::interface_configuration_type;
+
+namespace robotiq_2f_controllers
+{
+RobotiqForwardController::RobotiqForwardController() : controller_interface::ControllerInterface() {}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_init()
+{
+    joint_name_ = auto_declare<std::string>("joint", joint_name_);
+    command_interface_types_ =
+      auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration()
+  const
+{
+    controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
+
+    conf.names.reserve(command_interface_types_.size());
+    for (const auto & interface_type : command_interface_types_)
+    {
+        conf.names.push_back(joint_name_ + "/" + interface_type);
+    }
+
+    return conf;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &)
+{
+    auto callback =
+        [this](const std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> cmd_msg) -> void
+        {
+            msg_external_ptr_.writeFromNonRT(cmd_msg);
+            new_msg_ = true;
+        };
+
+    command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
+        "~/command", rclcpp::SystemDefaultsQoS(), callback);
+
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &)
+{
+  // clear out vectors in case of restart
+  joint_position_command_interface_.clear();
+  max_velocity_command_interface_.clear();
+  max_effort_command_interface_.clear();
+
+  // assign command interfaces
+  for (auto & interface : command_interfaces_)
+  {
+    command_interface_map_[interface.get_interface_name()]->push_back(interface);
+  }
+
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    if (new_msg_)
+    {
+        command_msg_ = *msg_external_ptr_.readFromRT();
+        desired_position_ = command_msg_->position;
+        desired_max_velocity_ = command_msg_->max_velocity;
+        desired_max_effort_ = command_msg_->max_effort;
+    }
+    // Set the hardware interface commands
+    joint_position_command_interface_[0].get().set_value(desired_position_);
+    max_velocity_command_interface_[0].get().set_value(desired_max_velocity_);
+    max_effort_command_interface_[0].get().set_value(desired_max_effort_);
+
+    return controller_interface::return_type::OK;
+}
+
+
+controller_interface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State &)
+{
+  release_interfaces();
+
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_error(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+}

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller_old.cpp

@@ -0,0 +1,82 @@
+#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"
+#include <pluginlib/class_list_macros.hpp>
+
+namespace robotiq_2f_controllers
+{
+RobotiqForwardController::RobotiqForwardController()
+{
+
+}
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
+{
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+    config.names.emplace_back(joint_name_ + hardware_interface::HW_IF_POSITION);
+    config.names.emplace_back(joint_name_ + "/set_gripper_max_effort");
+    config.names.emplace_back(joint_name_ + "/set_gripper_max_velocity");
+    return config;
+}
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const
+{
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::NONE;
+    return config;
+}
+
+void RobotiqForwardController::command_callback(const robotiq_2f_msgs::msg::ForwardCommand::SharedPtr msg)
+{
+    desired_position_ = msg->position;
+    desired_max_velocity_ = msg->max_velocity;
+    desired_max_effort_ = msg->max_effort;
+}
+
+controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    // Set the hardware interface commands
+    position_command_.set_value(desired_position_);
+    max_effort_command_.set_value(desired_max_effort_);
+    max_velocity_command_.set_value(desired_max_velocity_);
+
+    return controller_interface::return_type::OK;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_init()
+{
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+    joint_name_ = get_node()->declare_parameter<std::string>("joint_name", "finger_joint");
+
+    auto command_interfaces = get_node()->get_node_base_interface();
+    auto position_handle = robot_hardware.get_handle(joint_name_ + hardware_interface::HW_IF_POSITION);
+
+
+    position_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].);
+    max_effort_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].);
+    max_velocity_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_(joint_name_ + "/set_gripper_max_velocity"));
+
+
+    command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
+        "~/command", 10, std::bind(&RobotiqForwardController::command_callback, this, std::placeholders::_1));
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+    for (size_t i = 0; i < command_interfaces_.size(); ++i) {
+        command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
+    }
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+}  // namespace robotiq_2f_controllers
+
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro

@@ -19,7 +19,8 @@
             <!-- Plugins -->
             <hardware>
                  <xacro:if value="${use_fake_hardware}">
-                    <plugin>mock_components/GenericSystem</plugin>
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
+                    <param name="use_fake_hardware">${use_fake_hardware}</param>
                     <param name="mock_sensor_commands">${fake_sensor_commands}</param>
                     <param name="state_following_offset">0.0</param>
                 </xacro:if>
@@ -29,6 +30,7 @@
                     <param name="robot_ip">${robot_ip}</param>
                     <param name="robot_port">${robot_port}</param>
                     <param name="use_fake_hardware">${use_fake_hardware}</param>
+                </xacro:unless>
                 <param name="gripper_speed_multiplier">1.0</param>
                 <param name="gripper_force_multiplier">0.5</param>
                 <param name="min_position">${min_position}</param>
@@ -37,7 +39,6 @@
                 <param name="max_speed">${max_speed}</param>
                 <param name="min_force">${min_force}</param>
                 <param name="max_force">${max_force}</param>
-                </xacro:unless>
             </hardware>
 
             <!-- Joint interfaces -->
@@ -50,7 +51,8 @@
                 <state_interface name="velocity"/>
             </joint>
             <!-- When simulating we need to include the rest of the gripper joints -->
-            <xacro:if value="${use_fake_hardware}">
+            <!-- <xacro:if value="${use_fake_hardware}"> -->
+            <xacro:if value="false">
                 <joint name="${prefix}right_outer_knuckle_joint">
                     <param name="mimic">${prefix}finger_joint</param>
                     <param name="multiplier">-1</param>

src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ForwardCommand.msg"
+  # Add more messages here
+  DEPENDENCIES builtin_interfaces
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

src/robotiq_2f/robotiq_2f_msgs/msg/ForwardCommand.msg

@@ -0,0 +1,3 @@
+float64 position
+float64 max_velocity
+float64 max_effort

src/robotiq_2f/robotiq_2f_msgs/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robotiq_2f_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>builtin_interfaces</depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp

@@ -105,17 +105,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Roboti
 bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
                                                     std_srvs::srv::Trigger::Response::SharedPtr resp)
 {
-  RCLCPP_INFO(get_node()->get_logger(), "Received reactivation request.");
   command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING);
   command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0);
-  RCLCPP_INFO(get_node()->get_logger(), "Reactivation command sent to hardware interface.");
 
-  int attempts = 0;
-  int max_attempts = 1000;
-  while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING && attempts < max_attempts) {
+  while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING)
+  {
     std::this_thread::sleep_for(std::chrono::milliseconds(50));
-      attempts++;
-      RCLCPP_INFO(get_node()->get_logger(), "Attempt %d: Checking response from hardware interface: %f", attempts, command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value());
   }
   resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value();
 

src/ur_robotiq_description/config/ur_robotiq_calibration.yaml

@@ -0,0 +1,44 @@
+kinematics:
+  shoulder:
+    x: 0
+    y: 0
+    z: 0.1519161231793138
+    roll: -0
+    pitch: 0
+    yaw: -8.3688888041777445e-08
+  upper_arm:
+    x: -9.8255319984929057e-05
+    y: 0
+    z: 0
+    roll: 1.5713473529266058
+    pitch: 0
+    yaw: 4.5372573280303068e-06
+  forearm:
+    x: -0.24361145101605494
+    y: 0
+    z: 0
+    roll: 3.138959939545904
+    pitch: 3.141417818890341
+    yaw: 3.1415861917625292
+  wrist_1:
+    x: -0.21328351386382516
+    y: -0.00058695912107010507
+    z: 0.13086135281159214
+    roll: 0.0044853210843803608
+    pitch: 0.00071590512460444685
+    yaw: -2.0286176909971606e-06
+  wrist_2:
+    x: 9.0086536104922084e-05
+    y: -0.085314349728116412
+    z: -6.1195228207677147e-05
+    roll: 1.5715136178239859
+    pitch: 0
+    yaw: 1.3915445690113049e-06
+  wrist_3:
+    x: -0.00014076969524819022
+    y: 0.092309359181826575
+    z: -0.00011628039579221433
+    roll: 1.5695366459205147
+    pitch: 3.1415926535897931
+    yaw: 3.1415925648453848
+  hash: calib_2535041911463403862