做家庭影院的有哪些网站,遵义seo快速排名,镇江核酸检测最新通知,档案安全网站安全建设.创建功能包和节点 这里我们设计两个节点
example_interfaces_robot_01#xff0c;机器人节点#xff0c;对外提供控制机器人移动服务并发布机器人的状态。 example_interfaces_control_01#xff0c;控制节点#xff0c;发送机器人移动请求#xff0c;订阅机器人状态话题…
.创建功能包和节点 这里我们设计两个节点
example_interfaces_robot_01机器人节点对外提供控制机器人移动服务并发布机器人的状态。 example_interfaces_control_01控制节点发送机器人移动请求订阅机器人状态话题。
创建节点
ros2 pkg create example_interfaces_rclcpp --build-type ament_cmake --dependencies rclcpp example_ros2_interfaces --destination-directory src --node-name example_interfaces_robot_01 touch src/example_interfaces_rclcpp/src/example_interfaces_control_01.cpp #include rclcpp/rclcpp.hpp #include example_ros2_interfaces/srv/move_robot.hpp #include example_ros2_interfaces/msg/robot_status.hpp class ExampleInterfacesControl : public rclcpp::Node { public: ExampleInterfacesControl(std::string name) : Node(name) { RCLCPP_INFO(this-get_logger(), 节点已启动%s., name.c_str()); /*创建move_robot客户端*/ client_ this-create_clientexample_ros2_interfaces::srv::MoveRobot( move_robot); /*订阅机器人状态话题*/ robot_status_subscribe_ this-create_subscriptionexample_ros2_interfaces::msg::RobotStatus(robot_status, 10, std::bind(ExampleInterfacesControl::robot_status_callback_, this, std::placeholders::_1)); } /** * brief 发送移动机器人请求函数 * 步骤1.等待服务上线 * 2.构造发送请求 * * param distance */ void move_robot(float distance) { RCLCPP_INFO(this-get_logger(), 请求让机器人移动%f, distance); /*等待服务端上线*/ while (!client_-wait_for_service(std::chrono::seconds(1))) { //等待时检测rclcpp的状态 if (!rclcpp::ok()) { RCLCPP_ERROR(this-get_logger(), 等待服务的过程中被打断...); return; } RCLCPP_INFO(this-get_logger(), 等待服务端上线中); } // 构造请求 auto request std::make_sharedexample_ros2_interfaces::srv::MoveRobot::Request(); request-distance distance; // 发送异步请求然后等待返回返回时调用回调函数 client_-async_send_request( request, std::bind(ExampleInterfacesControl::result_callback_, this, std::placeholders::_1)); };
private: // 声明客户端 rclcpp::Clientexample_ros2_interfaces::srv::MoveRobot::SharedPtr client_; rclcpp::Subscriptionexample_ros2_interfaces::msg::RobotStatus::SharedPtr robot_status_subscribe_; /* 机器人移动结果回调函数 */ void result_callback_( rclcpp::Clientexample_ros2_interfaces::srv::MoveRobot::SharedFuture result_future) { auto response result_future.get(); RCLCPP_INFO(this-get_logger(), 收到移动结果%f, response-pose); } /** * brief 机器人状态话题接收回调函数 * * param msg */ void robot_status_callback_(const example_ros2_interfaces::msg::RobotStatus::SharedPtr msg) { RCLCPP_INFO(this-get_logger(), 收到状态数据位置%f 状态%d, msg-pose ,msg-status); } }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node std::make_sharedExampleInterfacesControl(example_interfaces_control_01); /*这里调用了服务让机器人向前移动5m*/ node-move_robot(5.0); rclcpp::spin(node); rclcpp::shutdown(); return 0; } 编写机器人节点逻辑
example_interfaces_robot_01.cpp
#include example_ros2_interfaces/msg/robot_status.hpp #include example_ros2_interfaces/srv/move_robot.hpp #include rclcpp/rclcpp.hpp
/*创建一个机器人类模拟真实机器人*/ class Robot { public: Robot() default; ~Robot() default; /** * brief 移动指定的距离 * * param distance * return float */ float move_distance(float distance) { status_ example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING; target_pose_ distance; // 当目标距离和当前距离大于0.01则持续向目标移动 while (fabs(target_pose_ - current_pose_) 0.01) { // 每一步移动当前到目标距离的1/10 float step distance / fabs(distance) * fabs(target_pose_ - current_pose_) * 0.1; current_pose_ step; std::cout 移动了 step 当前位置 current_pose_ std::endl; // 当前线程休眠500ms std::this_thread::sleep_for(std::chrono::milliseconds(500)); } status_ example_ros2_interfaces::msg::RobotStatus::STATUS_STOP; return current_pose_; } /** * brief Get the current pose * * return float */ float get_current_pose() { return current_pose_; } /** * brief Get the status * * return int * 1 example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING * 2 example_ros2_interfaces::msg::RobotStatus::STATUS_STOP */ int get_status() { return status_; }
private: // 声明当前位置 float current_pose_ 0.0; // 目标距离 float target_pose_ 0.0; int status_ example_ros2_interfaces::msg::RobotStatus::STATUS_STOP; }; class ExampleInterfacesRobot : public rclcpp::Node { public: ExampleInterfacesRobot(std::string name) : Node(name) { RCLCPP_INFO(this-get_logger(), 节点已启动%s., name.c_str()); /*创建move_robot服务*/ move_robot_server_ this-create_serviceexample_ros2_interfaces::srv::MoveRobot( move_robot, std::bind(ExampleInterfacesRobot::handle_move_robot, this, std::placeholders::_1, std::placeholders::_2)); /*创建发布者*/ robot_status_publisher_ this-create_publisherexample_ros2_interfaces::msg::RobotStatus(robot_status, 10); /*创建一个周期为500ms的定时器*/ timer_ this-create_wall_timer(std::chrono::milliseconds(500), std::bind(ExampleInterfacesRobot::timer_callback, this)); }
private: Robot robot; /*实例化机器人*/ rclcpp::TimerBase::SharedPtr timer_; /*定时器用于定时发布机器人位置*/ rclcpp::Serviceexample_ros2_interfaces::srv::MoveRobot::SharedPtr move_robot_server_; /*移动机器人服务*/ rclcpp::Publisherexample_ros2_interfaces::msg::RobotStatus::SharedPtr robot_status_publisher_; /*发布机器人位姿发布者*/ /** * brief 500ms 定时回调函数 * */ void timer_callback() { // 创建消息 example_ros2_interfaces::msg::RobotStatus message; message.status robot.get_status(); message.pose robot.get_current_pose(); RCLCPP_INFO(this-get_logger(), Publishing: %f, robot.get_current_pose()); // 发布消息 robot_status_publisher_-publish(message); }; /** * brief 收到话题数据的回调函数 * * param request 请求共享指针包含移动距离 * param response 响应的共享指针包含当前位置信息 */ void handle_move_robot(const std::shared_ptrexample_ros2_interfaces::srv::MoveRobot::Request request, std::shared_ptrexample_ros2_interfaces::srv::MoveRobot::Response response) { RCLCPP_INFO(this-get_logger(), 收到请求移动距离%f当前位置%f, request-distance, robot.get_current_pose()); robot.move_distance(request-distance); response-pose robot.get_current_pose(); };
}; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node std::make_sharedExampleInterfacesRobot(example_interfaces_robot_01); rclcpp::spin(node); rclcpp::shutdown(); return 0; } 编译运行节点
colcon build --packages-up-to example_interfaces_rclcpp 控制端
source install/setup.bash ros2 run example_interfaces_rclcpp example_interfaces_control_01 服务端
source install/setup.bash ros2 run example_interfaces_rclcpp example_interfaces_robot_01
