Quella/ROS2_WS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

机器人项目完成

Browse Source
This commit is contained in:
Quella
2026-02-19 16:21:36 +08:00
parent 357934af9b
commit 5384cdb6ff
532 changed files with 70884 additions and 459 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
protocol_ws/virtual_robot_prococol/src/robot_control.cpp Normal file
View File

@@ -0,0 +1,163 @@
/*
本程序用于机器人控制,例如速度控制,复位等等
*/
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "virtual_robot_prococol/msg/robot_status.hpp"
#include "virtual_robot_prococol/msg/robot_command.hpp"
#include "virtual_robot_prococol/action/target_position.hpp"
#include "std_srvs/srv/empty.hpp"
using status = virtual_robot_prococol::msg::RobotStatus;
using command = virtual_robot_prococol::msg::RobotCommand;
using TargetPosition = virtual_robot_prococol::action::TargetPosition;
// 机器人控制类
class RobotControl : public rclcpp::Node
{
public:
RobotControl() : Node("robot_control")
{
// 订阅状态消息,打印机器人状态信息
subscription_ = this->create_subscription<status>(
"robot_status",
10,
std::bind(&RobotControl::status_callback, this, std::placeholders::_1));
// 发布控制命令,例如速度控制
publisher_ = this->create_publisher<command>("robot_command", 10);
reset_client_ = this->create_client<std_srvs::srv::Empty>("reset_robot");
action_client_ = rclcpp_action::create_client<TargetPosition>(this, "target_position");
capture_control_command(); // 捕获控制命令并发布
}
// 发送目标位置
void send_target_position(float target_x, float target_y)
{
auto goal_msg = TargetPosition::Goal();
goal_msg.target_x = target_x;
goal_msg.target_y = target_y;
// 发送目标位置,并设置回调函数
rclcpp_action::Client<TargetPosition>::SendGoalOptions options;
options.goal_response_callback = std::bind(&RobotControl::goal_response_callback, this, std::placeholders::_1);
options.feedback_callback = std::bind(&RobotControl::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
options.result_callback = std::bind(&RobotControl::result_callback, this, std::placeholders::_1);
// 异步发送目标位置请求，结果通过回调函数处理。
action_client_->async_send_goal(goal_msg, options);
}
// 目标位置发送,服务器回应回调函数
void goal_response_callback(rclcpp_action::ClientGoalHandle<TargetPosition>::SharedPtr goal_handle)
{
if (!goal_handle)
{
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by the action server");
}
else
{
RCLCPP_INFO(this->get_logger(), "Goal accepted by the action server, waiting for result");
}
}
// 目标位置实时进度回调函数
void feedback_callback(rclcpp_action::ClientGoalHandle<TargetPosition>::SharedPtr goal_handle, const std::shared_ptr<const TargetPosition::Feedback> feedback)
{
(void)goal_handle;
RCLCPP_INFO(this->get_logger(), "Received feedback: progress=%.2f", feedback->progress);
}
// 目标位置结果回调函数
void result_callback(const rclcpp_action::ClientGoalHandle<TargetPosition>::WrappedResult &result)
{
switch (result.code)
{
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "Goal succeeded: %s", result.result->msg.c_str());
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted: %s", result.result->msg.c_str());
break;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_WARN(this->get_logger(), "Goal was canceled: %s", result.result->msg.c_str());
break;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
break;
}
}
// 循环捕获控制命令,例如速度控制,并发布控制命令
void capture_control_command()
{
// 示例: 捕获键盘输入的速度控制命令
std::string input;
std::cout << "Enter control command ('x 1.0 y 0.5'): ";
/* code */
std::getline(std::cin, input);
// 解析输入命令并发布控制命令
auto cmd = command(); // 创建控制命令消息对象
// 解析输入并设置cmd的字段，例如cmd.speed_x和cmd.speed_y
// 这里需要根据实际输入格式进行解析: 'x 1.0 y 0.5'
std::istringstream iss(input);
std::string token;
while (iss >> token)
{
if (token == "x")
{
iss >> cmd.speed_x;
}
else if (token == "y")
{
iss >> cmd.speed_y;
}
else if (token == "reset")
{
// 发送复位命令
auto reset_msg = std::make_shared<std_srvs::srv::Empty::Request>();
reset_client_->async_send_request(reset_msg);
}
else if (token == "target")
{
float target_x, target_y;
iss >> target_x >> target_y;
send_target_position(target_x, target_y);
}
}
publisher_->publish(cmd); // 发布控制命令
}
// 打印状态消息
void status_callback(const status::SharedPtr msg)
{
RCLCPP_INFO(this->get_logger(), "Received Robot Status: pos_x=%.2f, pos_y=%.2f, speed_x=%.2f, speed_y=%.2f, time=%s",
msg->pos_x, msg->pos_y, msg->speed_x, msg->speed_y, msg->now_time.c_str());
}
private:
// 订阅状态消息,打印机器人状态信息 使用Topic订阅状态消息
rclcpp::Publisher<command>::SharedPtr publisher_;
rclcpp::Subscription<status>::SharedPtr subscription_;
// 重置机器人状态信息服务
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset_client_;
// 轨迹规划action:接受客户端的请求,将机器人移动至目标位置
rclcpp_action::Client<TargetPosition>::SharedPtr action_client_;
rclcpp::TimerBase::SharedPtr timer_; // 定时器类
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<RobotControl>());
rclcpp::shutdown();
return 0;
}

224
protocol_ws/virtual_robot_prococol/src/robot_status.cpp Normal file
View File

@@ -0,0 +1,224 @@
/*
本程序用于发布机器人的状态参数,包括速度,位置,时间戳
节点内部维护机器人状态
接收控制命令,更改机器人状态
*/
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "virtual_robot_prococol/msg/robot_status.hpp"
#include "virtual_robot_prococol/msg/robot_command.hpp"
#include "virtual_robot_prococol/action/target_position.hpp"
#include "std_srvs/srv/empty.hpp"
#include <chrono>
using status = virtual_robot_prococol::msg::RobotStatus;
using Command = virtual_robot_prococol::msg::RobotCommand;
using TargetPosition = virtual_robot_prococol::action::TargetPosition;
// 模拟传感器
struct RobotStatus
{
float pos_x = 0.0; // 位置X
float pos_y = 0.0; // 位置Y
float speed_x = 0.0; // 速度X
float speed_y = 0.0; // 速度Y
std::string now_time; // 当前时间戳
} robotStatus_;
// 机器人状态发布类
class RobotStatusPublisher : public rclcpp::Node
{
public:
RobotStatusPublisher() : Node("robot_status")
{
// 发布状态消息
publisher_ = this->create_publisher<status>("robot_status", 10);
timer_ = this->create_wall_timer(
std::chrono::seconds(1),
std::bind(&RobotStatusPublisher::publish_status, this));
// 订阅控制命令,更改机器人状态
subscription_ = this->create_subscription<Command>(
"robot_command",
10,
std::bind(&RobotStatusPublisher::command_callback, this, std::placeholders::_1));
// 创建复位机器人状态信息服务
reset_service_ = this->create_service<std_srvs::srv::Empty>(
"reset_robot",
std::bind(&RobotStatusPublisher::reset_callback, this, std::placeholders::_1, std::placeholders::_2));
// 创建轨迹规划action服务器
action_server_ = rclcpp_action::create_server<TargetPosition>(
this,
"target_position",
std::bind(&RobotStatusPublisher::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&RobotStatusPublisher::handle_cancel, this, std::placeholders::_1),
std::bind(&RobotStatusPublisher::handle_accepted, this, std::placeholders::_1));
}
// action 回调
// 是否受理客户端的目标位置请求
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const TargetPosition::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with target position (%.2f, %.2f)", goal->target_x, goal->target_y);
(void)uuid; // 避免未使用参数的编译警告
// 校验位置是否合法
if (goal->target_x < -10 || goal->target_x > 10 || goal->target_y < -10 || goal->target_y > 10)
{
RCLCPP_WARN(this->get_logger(), "Target position is out of bounds");
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
// 客户端主动取消轨迹规划
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<TargetPosition>> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle; // 避免未使用参数的编译警告
return rclcpp_action::CancelResponse::ACCEPT;
}
// 接受到目标位置请求后执行的回调
void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<TargetPosition>> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
// 在这里实现机器人移动到目标位置的逻辑
// 例如，可以创建一个新的线程来执行长时间运行的任务
std::thread{std::bind(&RobotStatusPublisher::execute_goal, this, std::placeholders::_1), goal_handle}.detach();
}
// 轨迹运动执行逻辑
void execute_goal(const std::shared_ptr<rclcpp_action::ServerGoalHandle<TargetPosition>> goal_handle)
{
// 这里是执行目标位置请求的具体逻辑
// 例如，可以模拟机器人移动到目标位置的过程，并定期发布反馈消息
// 当前进度
auto feedback = std::make_shared<TargetPosition::Feedback>();
feedback->progress = 0.0;
// 模拟移动过程
while (feedback->progress < 1.0)
{
// 如果节点被关闭
if (!rclcpp::ok())
{
RCLCPP_INFO(this->get_logger(), "Node is shutting down, aborting goal");
auto result = std::make_shared<TargetPosition::Result>();
result->msg = "Node is shutting down";
result->success = false;
result->status = TargetPosition::Result::STATUS_ABORTED;
goal_handle->abort(result);
return;
}
// 检查是否收到取消请求
if (goal_handle->is_canceling())
{
RCLCPP_INFO(this->get_logger(), "Goal canceled");
auto result = std::make_shared<TargetPosition::Result>();
result->msg = "Goal was canceled";
result->success = false;
result->status = TargetPosition::Result::STATUS_CANCELED;
goal_handle->canceled(result);
return;
}
// 获取目标位置
auto goal = goal_handle->get_goal();
// 计算当前进度（这里是一个简单的线性模拟，实际应用中可能需要根据机器人当前位置和目标位置计算）
feedback->progress += 0.1; // 模拟进度增加
if (feedback->progress > 1.0)
{
feedback->progress = 1.0;
robotStatus_.pos_x = goal->target_x; // 更新位置X
robotStatus_.pos_y = goal->target_y; // 更新位置Y
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
auto result = std::make_shared<TargetPosition::Result>();
result->msg = "Goal reached successfully";
result->success = true;
result->status = TargetPosition::Result::STATUS_SUCCEEDED;
goal_handle->succeed(result);
return;
}
// 发布反馈消息
goal_handle->publish_feedback(feedback);
}
}
void reset_callback(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response)
{
(void)request; // 避免未使用参数的编译警告
(void)response; // 避免未使用参数的编译警告
// 重置机器人状态信息
robotStatus_.pos_x = 0.0;
robotStatus_.pos_y = 0.0;
robotStatus_.speed_x = 0.0;
robotStatus_.speed_y = 0.0;
RCLCPP_INFO(this->get_logger(), "Robot status has been reset.");
}
// 更改机器人状态
void command_callback(const Command::SharedPtr msg)
{
robotStatus_.speed_x = msg->speed_x; // 更新速度X
robotStatus_.speed_y = msg->speed_y; // 更新速度Y
}
// 发布状态消息
void publish_status()
{
// 创建状态消息并发布
auto msg = status(); // 创建状态消息对象
msg.speed_x = robotStatus_.speed_x; // 示例速度X
msg.speed_y = robotStatus_.speed_y; // 示例速度Y
auto now = this->now();
auto dt = now - lastTime; // 计算时间差
lastTime = now; // 更新上次发布状态消息的时间
robotStatus_.pos_x += robotStatus_.speed_x * dt.seconds(); // 根据速度更新位置X
robotStatus_.pos_y += robotStatus_.speed_y * dt.seconds(); // 根据速度更新位置Y
msg.pos_x = robotStatus_.pos_x; // 示例位置X
msg.pos_y = robotStatus_.pos_y; // 示例位置Y
// 获取当前时间戳
msg.now_time = std::to_string(now.seconds()); // 将时间戳转换为字符串
publisher_->publish(msg);
}
private:
// 发布状态消息 使用Topic发布状态消息
rclcpp::Publisher<status>::SharedPtr publisher_;
rclcpp::Subscription<Command>::SharedPtr subscription_;
// 复位机器人状态信息服务
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_service_;
// 轨迹规划action:接受客户端的请求,将机器人移动至目标位置
rclcpp_action::Server<TargetPosition>::SharedPtr action_server_;
// 定时器用于定期发布状态消息
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time lastTime = this->now(); // 上次发布状态消息的时间
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<RobotStatusPublisher>());
rclcpp::shutdown();
return 0;
}