.. _program_listing_file__tmp_ws_src_mavros_mavros_extras_include_mavros_extras_servo_state_publisher.hpp:

Program Listing for File servo_state_publisher.hpp
==================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_mavros_mavros_extras_include_mavros_extras_servo_state_publisher.hpp>` (``/tmp/ws/src/mavros/mavros_extras/include/mavros_extras/servo_state_publisher.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Copyright 2021 Vladimir Ermakov.
    *
    * This file is part of the mavros package and subject to the license terms
    * in the top-level LICENSE file of the mavros repository.
    * https://github.com/mavlink/mavros/tree/master/LICENSE.md
    */
   #pragma once
   
   #ifndef MAVROS_EXTRAS__SERVO_STATE_PUBLISHER_HPP_
   #define MAVROS_EXTRAS__SERVO_STATE_PUBLISHER_HPP_
   
   #include <yaml-cpp/yaml.h>
   #include <urdf/model.h>
   
   #include <algorithm>
   #include <memory>
   #include <string>
   #include <list>
   #include <shared_mutex>  // NOLINT
   
   #include "rclcpp/macros.hpp"
   #include "rclcpp/rclcpp.hpp"
   
   #include "std_msgs/msg/string.hpp"
   #include "mavros_msgs/msg/rc_out.hpp"
   #include "sensor_msgs/msg/joint_state.hpp"
   
   namespace mavros
   {
   namespace extras
   {
   
   class ServoDescription
   {
   public:
     std::string joint_name;
     float joint_lower;
     float joint_upper;
   
     size_t rc_channel;
   
     uint16_t rc_min;
     uint16_t rc_max;
     uint16_t rc_trim;
     uint16_t rc_dz;
     bool rc_rev;
   
     explicit ServoDescription(std::string joint_name_ = {})
     : joint_name(joint_name_),
       joint_lower(-M_PI / 4),
       joint_upper(M_PI / 4),
       rc_channel(0),
       rc_min(1000),
       rc_max(2000),
       rc_trim(1500),
       rc_dz(0),
       rc_rev(false)
     {}
   
     ServoDescription(urdf::Model & model, std::string joint_name_, YAML::Node config);
   
     inline float normalize(uint16_t pwm)
     {
       // 1) fix bounds
       pwm = std::max(pwm, rc_min);
       pwm = std::min(pwm, rc_max);
   
       // 2) scale around mid point
       float chan;
       if (pwm > (rc_trim + rc_dz)) {
         chan = (pwm - rc_trim - rc_dz) / static_cast<float>(rc_max - rc_trim - rc_dz);
       } else if (pwm < (rc_trim - rc_dz)) {
         chan = (pwm - rc_trim + rc_dz) / static_cast<float>(rc_trim - rc_min - rc_dz);
       } else {
         chan = 0.0;
       }
   
       if (rc_rev) {
         chan *= -1;
       }
   
       if (!std::isfinite(chan)) {
         chan = 0.0;
       }
   
       return chan;
     }
   
     inline float calculate_position(uint16_t pwm)
     {
       float channel = normalize(pwm);
   
       // not sure should i differently map -1..0 and 0..1
       // for now there arduino map() (explicit)
       float position = (channel + 1.0) * (joint_upper - joint_lower) / (1.0 + 1.0) + joint_lower;
   
       return position;
     }
   };
   
   class ServoStatePublisher : public rclcpp::Node
   {
   public:
     explicit ServoStatePublisher(const std::string & node_name = "servo_state_publisher")
     : ServoStatePublisher(rclcpp::NodeOptions(), node_name) {}
   
     explicit ServoStatePublisher(
       const rclcpp::NodeOptions & options,
       const std::string & node_name = "servo_state_publisher");
   
   private:
     rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_sub;
     rclcpp::Subscription<mavros_msgs::msg::RCOut>::SharedPtr rc_out_sub;
     rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_states_pub;
   
     std::shared_mutex mutex;
     std::list<ServoDescription> servos;
   
     void robot_description_cb(const std_msgs::msg::String::SharedPtr msg);
     void rc_out_cb(const mavros_msgs::msg::RCOut::SharedPtr msg);
   };
   
   }   // namespace extras
   }   // namespace mavros
   
   #endif  // MAVROS_EXTRAS__SERVO_STATE_PUBLISHER_HPP_