ros2订阅esp32发布的电池电压数据

Posted 2022-08-09 zhangrelay

一个最简单的订阅和发布案例如下：

pub-

 

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"


using namespace std::chrono_literals;


int main(int argc, char * argv[])

  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("simple_publisher");
  auto pub = node->create_publisher<std_msgs::msg::String>("/my_message", 10);

  std_msgs::msg::String myMessage;
  size_t counter0;
  rclcpp::WallRate loop_rate(500ms);

  while (rclcpp::ok())
  
    myMessage.data = "Hello, ros2 world! " + std::to_string(counter++);
    RCLCPP_INFO(node->get_logger(), "Publishing: '%s'", myMessage.data.c_str());
    try
    
      pub->publish(myMessage);
      rclcpp::spin_some(node);
     catch (const rclcpp::exceptions::RCLError & e)
    
      RCLCPP_ERROR(node->get_logger(), "Errore type : %s", e.what());
    
    loop_rate.sleep();
  
  rclcpp::shutdown();
  return 0;

sub-

 

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

std::shared_ptr<rclcpp::Node> node = nullptr;

void TopicCallback(const std_msgs::msg::String::SharedPtr msg)

  RCLCPP_INFO(node->get_logger(), "I heard the message : '%s'", msg->data.c_str());


int main(int argc, char *argv[])

  rclcpp::init(argc, argv);
  node = std::make_shared<rclcpp::Node>("simple_subscriber");
  auto sub = node->create_subscription<std_msgs::msg::String>("/my_message", 10, TopicCallback);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;

---

esp32参考：

esp32发布机器人电池电压到ros2（micro-ros+CoCube）

---

数据类型是float32，需要修改，开启float32发布和订阅：

发布

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"


using namespace std::chrono_literals;


int main(int argc, char * argv[])

  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("simple_publisher");
  auto pub = node->create_publisher<std_msgs::msg::Float32>("/my_message", 10);

  std_msgs::msg::Float32 myMessage;
  rclcpp::WallRate loop_rate(500ms);

  while (rclcpp::ok())
  
    myMessage.data++;
    RCLCPP_INFO(node->get_logger(), "Publishing: '%f'", myMessage.data);
    try
    
      pub->publish(myMessage);
      rclcpp::spin_some(node);
     catch (const rclcpp::exceptions::RCLError & e)
    
      RCLCPP_ERROR(node->get_logger(), "Errore type : %s", e.what());
    
    loop_rate.sleep();
  
  rclcpp::shutdown();
  return 0;

 

订阅

 

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"

std::shared_ptr<rclcpp::Node> node = nullptr;

void TopicCallback(const std_msgs::msg::Float32::SharedPtr msg)

  RCLCPP_INFO(node->get_logger(), "I heard the message : '%f'", msg->data);


int main(int argc, char *argv[])

  rclcpp::init(argc, argv);
  node = std::make_shared<rclcpp::Node>("simple_subscriber");
  auto sub = node->create_subscription<std_msgs::msg::Float32>("/my_message", 10, TopicCallback);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;

 

---

再稍作修改，可以得到订阅esp32机器人电池电压的数据：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"

std::shared_ptr<rclcpp::Node> node = nullptr;

void TopicCallback(const std_msgs::msg::Float32::SharedPtr msg)

  RCLCPP_INFO(node->get_logger(), "Robot battery voltage : '%f'", msg->data);


int main(int argc, char *argv[])

  rclcpp::init(argc, argv);
  node = std::make_shared<rclcpp::Node>("battery_sub");
  auto sub = node->create_subscription<std_msgs::msg::Float32>("/robot_battery", 1, TopicCallback);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;

还遇到一些小问题^_^

#include <cstdio>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/string.hpp"

#include "demo_nodes_cpp/visibility_control.h"

namespace demo_nodes_cpp

class ListenerBestEffort : public rclcpp::Node

public:
  DEMO_NODES_CPP_PUBLIC
  explicit ListenerBestEffort(const rclcpp::NodeOptions & options)
  : Node("listener", options)
  
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
    auto callback =
      [this](std_msgs::msg::String::ConstSharedPtr msg) -> void
      
        RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str());
      ;

    sub_ = create_subscription<std_msgs::msg::String>("chatter", rclcpp::SensorDataQoS(), callback);
  

private:
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
;

  // namespace demo_nodes_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ListenerBestEffort)

---