Method cmd_vel_callback

turtlebot3_node/src/turtlebot3.cpp:331–406

}

void TurtleBot3::cmd_vel_callback()
{
  auto qos = rclcpp::QoS(rclcpp::KeepLast(10));
  cmd_vel_sub_ = std::make_unique<TwistSubscriber>(
    node_handle_,
    "cmd_vel",
    qos,
    std::function<void(const geometry_msgs::msg::Twist::SharedPtr)>(
      [this](const geometry_msgs::msg::Twist::SharedPtr msg) -> void
      {
        std::string sdk_msg;

        union Data {
          int32_t dword[6];
          uint8_t byte[4 * 6];
        } data;

        data.dword[0] = static_cast<int32_t>(msg->linear.x * 100);
        data.dword[1] = 0;
        data.dword[2] = 0;
        data.dword[3] = 0;
        data.dword[4] = 0;
        data.dword[5] = static_cast<int32_t>(msg->angular.z * 100);

        uint16_t start_addr = extern_control_table.cmd_velocity_linear_x.addr;
        uint16_t addr_length =
        (extern_control_table.cmd_velocity_angular_z.addr -
        extern_control_table.cmd_velocity_linear_x.addr) +
        extern_control_table.cmd_velocity_angular_z.length;

        uint8_t * p_data = &data.byte[0];

        dxl_sdk_wrapper_->set_data_to_device(start_addr, addr_length, p_data, &sdk_msg);

        RCLCPP_DEBUG(
          this->get_logger(),
          "lin_vel: %f ang_vel: %f msg : %s", msg->linear.x, msg->angular.z, sdk_msg.c_str());
      }
    ),
    std::function<void(const geometry_msgs::msg::TwistStamped::SharedPtr)>(
      [this](const geometry_msgs::msg::TwistStamped::SharedPtr msg) -> void
      {
        std::string sdk_msg;

        union Data {
          int32_t dword[6];
          uint8_t byte[4 * 6];
        } data;

        data.dword[0] = static_cast<int32_t>(msg->twist.linear.x * 100);
        data.dword[1] = 0;
        data.dword[2] = 0;
        data.dword[3] = 0;
        data.dword[4] = 0;
        data.dword[5] = static_cast<int32_t>(msg->twist.angular.z * 100);

        uint16_t start_addr = extern_control_table.cmd_velocity_linear_x.addr;
        uint16_t addr_length =