ROS2学习之路之main函数

最新推荐文章于 2024-06-28 17:19:39 发布

.小墨迹

最新推荐文章于 2024-06-28 17:19:39 发布

阅读量172

点赞数

分类专栏： ROS2学习文章标签：学习 c++ 算法

本文链接：https://blog.csdn.net/qq1240268067/article/details/132627879

版权

ROS2学习专栏收录该内容

13 篇文章 0 订阅

订阅专栏

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int8.hpp>
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "std_msgs/msg/string.hpp"
#include "msg_srv/srv/test.hpp"
#include "msg_srv/msg/test.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_1;

class MinimalNode
{
  private:
    rclcpp::Node::SharedPtr nodePtr_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;//发布

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_,subscription1_,subscription2_;//订阅
    rclcpp::Service<msg_srv::srv::Test>::SharedPtr  Srv1_,Srv2_;//服务端
    rclcpp::Client<msg_srv::srv::Test>::SharedPtr  client1_,client2_;//客户端

    rclcpp::CallbackGroup::SharedPtr callBackGroup_;
    rclcpp::CallbackGroup::SharedPtr srvCallBackGroup_;
    rclcpp::CallbackGroup::SharedPtr ClientCallBackGroup_;

  public:
    MinimalNode(rclcpp::Node::SharedPtr& nodePtr):nodePtr_(nodePtr)
    {
        callBackGroup_      = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
        srvCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
        ClientCallBackGroup_   = nodePtr_->create_callback_group(rclcpp::CallbackGroupType::Reentrant);//重入（Reentrant：每时刻允许多个线程） 互斥（MutuallyExclusive：每时刻只允许1个线程）
        
        rclcpp::SubscriptionOptions options;
        options.callback_group     = callBackGroup_;
        
        publisher_ = nodePtr_->create_publisher<std_msgs::msg::String>("topic", 10);//publisher_->publish(message);
        
        subscription_  = nodePtr_->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalNode::topic_callback, this, _1),options);
        subscription1_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic1", 10, std::bind(&MinimalNode::topic_callback1, this, _1),options);
        subscription2_ = nodePtr_->create_subscription<std_msgs::msg::String>("topic2", 10, std::bind(&MinimalNode::topic_callback2, this, _1),options);

        Srv1_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv1", std::bind(&MinimalNode::srv_callback1, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
        Srv2_ = nodePtr_->create_service<msg_srv::srv::Test>("Srv2", std::bind(&MinimalNode::srv_callback2, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, srvCallBackGroup_);
        
        client1_ = nodePtr_->create_client<msg_srv::srv::Test>("Srv1", rmw_qos_profile_default, ClientCallBackGroup_);
        client2_ = nodePtr_->create_client<msg_srv::srv::Test>("Srv2", rmw_qos_profile_default, ClientCallBackGroup_);
    }

    void topic_callback1(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'", msg->data.c_str());}
    void topic_callback2(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'", msg->data.c_str());}
    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s' ", msg->data.c_str());} 
    bool srv_callback1(const std::shared_ptr<msg_srv::srv::Test::Request> req, std::shared_ptr<msg_srv::srv::Test::Response> res) const
    {
      std::vector<int> num(req->goal_ids);
      std::string id = req->id;
      res->success = false;
      RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'  call success!!!", req->id.c_str());
      return false;
    }
    bool srv_callback2(const std::shared_ptr<msg_srv::srv::Test::Request> req, std::shared_ptr<msg_srv::srv::Test::Response> res) const
    {
      std::vector<int> num(req->goal_ids);
      std::string id = req->id;
      res->success = false;
      RCLCPP_INFO(nodePtr_->get_logger(), "I heard: '%s'  call success!!!", req->id.c_str());
      return false;
    }
    void run()
    {
      std::thread{[this]{
            rclcpp::WallRate loop_rate(1.0);//1Hz
            while (rclcpp::ok())
            {
                auto start = std::chrono::system_clock::now();
                auto request = std::make_shared<msg_srv::srv::Test::Request>();
                // auto Response = std::make_shared<msg_srv::srv::Test::Response>();
                request-> goal_ids.push_back(1);
                request-> id = "srv1-client1";
                auto result = client1_->async_send_request(request); 
                auto response = result.get();
                loop_rate.sleep();
            }
        }}.detach();
    }
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);

  MinimalNode vcq(nodePtr);
  vcq.run();
  
  auto exec_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(rclcpp::executor::create_default_executor_arguments(), 5);
  exec_->add_node(nodePtr);
  exec_->spin();
  exec_->remove_node(nodePtr);

  rclcpp::shutdown();
  return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(test)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wno-unused-parameter -Wextra -Wpedantic)
endif()

set(CMAKE_BUILD_TYPE Release)
add_definitions(${PCL_DEFINITIONS})

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)

find_package(PCL REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(message_filters REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(Boost REQUIRED)
find_package(msg_srv REQUIRED)


include_directories(include  ${geometry_msgs_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR})

set(DEPS
    msg_srv
    geometry_msgs
    message_filters
    pcl_conversions
    rclcpp
    sensor_msgs
    std_msgs
    nav_msgs
    Boost
    laser_geometry)
ament_export_include_directories(include)

# launch
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

# 引用头文件
include_directories(include/${PROJECT_NAME})

# 生成可执行文件
add_executable(${PROJECT_NAME}_node main.cpp src/test.cpp)
ament_target_dependencies(${PROJECT_NAME}_node ${DEPS} msg_srv sineva_nav)
target_link_libraries(${PROJECT_NAME}_node ${OCTOMAP_LIBRARIES} )#如果链接不上库就在这里添加链接库

# Timer
install(DIRECTORY include/ DESTINATION include)# 让自己的库可以被自己其他的包调用

install(
  TARGETS ${PROJECT_NAME}_node
  EXPORT export_${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME})

install(
  TARGETS ${PROJECT_NAME}_node
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://xxxx" schematypens="http://xxx"?>
<package format="3">
  <name>test</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="xmj@sineva.com.cn">xmj</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
    <depend>rclcpp</depend>
    <depend>std_msgs</depend>
    <depend>msg_srv</depend>
    <build_depend>sensor_msgs</build_depend>
    <run_depend>sensor_msgs</run_depend>
    <build_depend>laser_geometry</build_depend>
    <run_depend>laser_geometry</run_depend>

  </export>
</package>