柚子快報(bào)邀請碼778899分享：ROS2開發(fā)機(jī)器人移動

http://yzkb.51969.com/

.創(chuàng)建功能包和節(jié)點(diǎn) 這里我們設(shè)計(jì)兩個(gè)節(jié)點(diǎn)

example_interfaces_robot_01，機(jī)器人節(jié)點(diǎn)，對外提供控制機(jī)器人移動服務(wù)并發(fā)布機(jī)器人的狀態(tài)。 example_interfaces_control_01，控制節(jié)點(diǎn)，發(fā)送機(jī)器人移動請求，訂閱機(jī)器人狀態(tài)話題。

創(chuàng)建節(jié)點(diǎn)

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(), "節(jié)點(diǎn)已啟動：%s.", name.c_str()); ? ? /*創(chuàng)建move_robot客戶端*/ ? ? client_ = this->create_client( ? ? ? "move_robot"); ? ? /*訂閱機(jī)器人狀態(tài)話題*/ ? ? robot_status_subscribe_ = this->create_subscription("robot_status", 10, std::bind(&ExampleInterfacesControl::robot_status_callback_, this, std::placeholders::_1)); ? }

? /** ? ?* @brief 發(fā)送移動機(jī)器人請求函數(shù) ? ?* 步驟：1.等待服務(wù)上線 ? ?* ? ? ?2.構(gòu)造發(fā)送請求 ? ?*? ? ?* @param distance? ? ?*/ ? void move_robot(float distance) { ? ? RCLCPP_INFO(this->get_logger(), "請求讓機(jī)器人移動%f", distance);

? ? /*等待服務(wù)端上線*/ ? ? while (!client_->wait_for_service(std::chrono::seconds(1))) { ? ? ? //等待時(shí)檢測rclcpp的狀態(tài) ? ? ? if (!rclcpp::ok()) { ? ? ? ? RCLCPP_ERROR(this->get_logger(), "等待服務(wù)的過程中被打斷..."); ? ? ? ? return; ? ? ? } ? ? ? RCLCPP_INFO(this->get_logger(), "等待服務(wù)端上線中"); ? ? }

? ? // 構(gòu)造請求 ? ? auto request =? ? ? ? std::make_shared(); ? ? request->distance = distance;

? ? // 發(fā)送異步請求，然后等待返回，返回時(shí)調(diào)用回調(diào)函數(shù) ? ? client_->async_send_request( ? ? ? request, std::bind(&ExampleInterfacesControl::result_callback_, this, ? ? ? ? ? ? ? ? ? ? ? ? ?std::placeholders::_1)); ? };

private: ? // 聲明客戶端 ? rclcpp::Client::SharedPtr client_; ? rclcpp::Subscription::SharedPtr robot_status_subscribe_; ? /* 機(jī)器人移動結(jié)果回調(diào)函數(shù) */ ? void result_callback_( ? ? rclcpp::Client::SharedFuture ? ? ? result_future) { ? ? auto response = result_future.get(); ? ? RCLCPP_INFO(this->get_logger(), "收到移動結(jié)果：%f", response->pose); ? }

? /** ? ?* @brief 機(jī)器人狀態(tài)話題接收回調(diào)函數(shù) ? ?*? ? ?* @param msg? ? ?*/ ? void robot_status_callback_(const example_ros2_interfaces::msg::RobotStatus::SharedPtr msg) ? { ? ? RCLCPP_INFO(this->get_logger(), "收到狀態(tài)數(shù)據(jù)位置：%f 狀態(tài)：%d", msg->pose ,msg->status); ? }

};

int main(int argc, char** argv) { ? rclcpp::init(argc, argv); ? auto node = std::make_shared("example_interfaces_control_01"); ? /*這里調(diào)用了服務(wù)，讓機(jī)器人向前移動5m*/ ? node->move_robot(5.0); ? rclcpp::spin(node); ? rclcpp::shutdown(); ? return 0; }

編寫機(jī)器人節(jié)點(diǎn)邏輯

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"

/*創(chuàng)建一個(gè)機(jī)器人類，模擬真實(shí)機(jī)器人*/ 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; ? ? // 當(dāng)目標(biāo)距離和當(dāng)前距離大于0.01則持續(xù)向目標(biāo)移動 ? ? while (fabs(target_pose_ - current_pose_) > 0.01) { ? ? ? // 每一步移動當(dāng)前到目標(biāo)距離的1/10 ? ? ? float step = distance / fabs(distance) * fabs(target_pose_ - current_pose_) * 0.1; ? ? ? current_pose_ += step; ? ? ? std::cout << "移動了：" << step << "當(dāng)前位置：" << current_pose_ << std::endl; ? ? ? // 當(dāng)前線程休眠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: ? // 聲明當(dāng)前位置 ? float current_pose_ = 0.0; ? // 目標(biāo)距離 ? 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(), "節(jié)點(diǎn)已啟動：%s.", name.c_str()); ? ? /*創(chuàng)建move_robot服務(wù)*/ ? ? move_robot_server_ = this->create_service( ? ? ? "move_robot", std::bind(&ExampleInterfacesRobot::handle_move_robot, this, std::placeholders::_1, std::placeholders::_2)); ? ? /*創(chuàng)建發(fā)布者*/ ? ? robot_status_publisher_ = this->create_publisher("robot_status", 10); ? ? /*創(chuàng)建一個(gè)周期為500ms的定時(shí)器*/ ? ? timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&ExampleInterfacesRobot::timer_callback, this)); ? }

private: ? Robot robot; /*實(shí)例化機(jī)器人*/ ? rclcpp::TimerBase::SharedPtr timer_; /*定時(shí)器，用于定時(shí)發(fā)布機(jī)器人位置*/ ? rclcpp::Service::SharedPtr move_robot_server_; /*移動機(jī)器人服務(wù)*/ ? rclcpp::Publisher::SharedPtr robot_status_publisher_; /*發(fā)布機(jī)器人位姿發(fā)布者*/

? /** ? ?* @brief 500ms 定時(shí)回調(diào)函數(shù)， ? ?*? ? ?*/ ? void timer_callback() { ? ? // 創(chuàng)建消息 ? ? 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()); ? ? // 發(fā)布消息 ? ? robot_status_publisher_->publish(message); ? };

? /** ? ?* @brief 收到話題數(shù)據(jù)的回調(diào)函數(shù) ? ?*? ? ?* @param request 請求共享指針，包含移動距離 ? ?* @param response 響應(yīng)的共享指針，包含當(dāng)前位置信息 ? ?*/ ? void handle_move_robot(const std::shared_ptr request, ? ? ? ? ? ? ? ? ? ? ? ? ?std::shared_ptr response) { ? ? RCLCPP_INFO(this->get_logger(), "收到請求移動距離：%f，當(dāng)前位置：%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_shared("example_interfaces_robot_01"); ? ? rclcpp::spin(node); ? ? rclcpp::shutdown(); ? ? return 0; ? }

編譯運(yùn)行節(jié)點(diǎn)

colcon build --packages-up-to example_interfaces_rclcpp

控制端

source install/setup.bash ros2 run example_interfaces_rclcpp example_interfaces_control_01 ?

服務(wù)端

source install/setup.bash ros2 run example_interfaces_rclcpp ?example_interfaces_robot_01

柚子快報(bào)邀請碼778899分享：ROS2開發(fā)機(jī)器人移動

http://yzkb.51969.com/

精彩文章