基础
ros2 run turtlesim turtlesim_node //运行乌龟节点 ros2 node list //查询所有运行的节点 ros2 node info /turtlesim //查询乌龟节点的信息 //可发现 乌龟节点订阅了 /turtle1/cmd_vel //话题 消息接口是 geometry_msgs/msg/Twist //同时 乌龟节点 发布了一个话题 来输出自己的位置 //话题 /turtle1/pose 消息接口 turtlesim/msg/pose流程
创建包 并添加 geometry_msg turtlesim 依赖
ros2 pkg create demo_cpp_topic --build-type ament_cmake --dependencies rclcpp geometry_msgs turtlesim --license Apache-2.0在包下的src下编写turtle_circle.cpp
#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" #include <chrono> using namespace std::chrono_literals; class TurtleCircle : public rclcpp::Node{ private: rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_; public: explicit TurtleCircle(const std::string& node_name):Node(node_name){ publisher_=this->create_publisher<geometry_msgs::msg::Twist>( "/turtle1/cmd_vel",10); //相比py的简单粗暴 cpp需要bind将函数变成可直接调用的回调函数 timer_=this->create_wall_timer( 1000ms,std::bind(&TurtleCircle::timer_callback,this)); } private: void timer_callback(){ auto msg = geometry_msgs::msg::Twist(); msg.linear.x=1.0; msg.angular.z=0.5; publisher_->publish(msg); } }; int main(int argc ,char**argv){ rclcpp::init(argc,argv); auto node=std::make_shared<TurtleCircle>("thrtle_circle"); rclcpp::spin(node); rclcpp::shutdown(); return 0; }编写完成之后在CMakeLists.txt中添加节点 ，依赖之后构建项目就能运行了（当然要先source）
再开启乌龟节点 就可以看到乌龟转圈了
同理 编写 turtle_control.cpp 可以让乌龟向目标位置前进
#include "geometry_msgs/msg/twist.hpp" #include "rclcpp/rclcpp.hpp" #include "turtlesim/msg/pose.hpp" class TurtleController:public rclcpp::Node{ private: rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscription_; rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_; double target_x_{1.0}; double target_y_{1.0}; double k_{1.0}; double max_speed_{3.0}; private: void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose){ auto message=geometry_msgs::msg::Twist(); double current_x =pose->x; double current_y=pose->y; RCLCPP_INFO(this->get_logger(),"now location:(x=%f,y=%f)", current_x,current_y); double distance = std:: sqrt((target_x_-current_x)*(target_x_-current_x)+ (target_y_-current_y)*(target_y_-current_y)); double angle = std::atan2(target_y_-current_y,target_x_-current_x)-pose->theta; if(distance>0.1){ if(fabs(angle)>0.2){ message.angular.z=fabs(angle); }else{ message.linear.x=k_ * distance; } } if(message.linear.x>max_speed_){ message.linear.x=max_speed_; } velocity_publisher_->publish(message); } public: TurtleController():Node("turtle_controller"){ velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>( "/turtle1/cmd_vel",10); pose_subscription_=this->create_subscription<turtlesim::msg::Pose>( "turtle1/pose",10,std::bind(&TurtleController::on_pose_received_,this, std::placeholders::_1)); } }; int main(int argc ,char** argv){ rclcpp::init(argc,argv); auto node=std::make_shared<TurtleController>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }
