#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>