ROS2开发机器人移动

news2024/11/26 8:32:11

.创建功能包和节点
这里我们设计两个节点

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_client<example_ros2_interfaces::srv::MoveRobot>(
"move_robot");
/*订阅机器人状态话题*/
robot_status_subscribe_ = this->create_subscription<example_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_shared<example_ros2_interfaces::srv::MoveRobot::Request>();
request->distance = distance;

// 发送异步请求，然后等待返回，返回时调用回调函数
client_->async_send_request(
request, std::bind(&ExampleInterfacesControl::result_callback_, this,
std::placeholders::_1));
};

private:
// 声明客户端
rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedPtr client_;
rclcpp::Subscription<example_ros2_interfaces::msg::RobotStatus>::SharedPtr robot_status_subscribe_;
/* 机器人移动结果回调函数 */
void result_callback_(
rclcpp::Client<example_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_shared<ExampleInterfacesControl>("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_service<example_ros2_interfaces::srv::MoveRobot>(
"move_robot", std::bind(&ExampleInterfacesRobot::handle_move_robot, this, std::placeholders::_1, std::placeholders::_2));
/*创建发布者*/
robot_status_publisher_ = this->create_publisher<example_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::Service<example_ros2_interfaces::srv::MoveRobot>::SharedPtr move_robot_server_; /*移动机器人服务*/
rclcpp::Publisher<example_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_ptr<example_ros2_interfaces::srv::MoveRobot::Request> request,
std::shared_ptr<example_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_shared<ExampleInterfacesRobot>("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