Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

删除 Cyberdog_demo 子项目文件

Browse Source
This commit is contained in:
Havoc 2025-07-05 22:31:28 +08:00
parent 4b34cd5d33
commit a43c287e65
11 changed files with 605 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
test/Cyberdog_demo

@ -1 +0,0 @@
Subproject commit b58deea063207408e002eba06fc76207ccaeef3b

2
test/cyberdog_demo_test/README.md Normal file
View File

@ -0,0 +1,2 @@
# Cyberdog_demo
cyberdog example

51
test/cyberdog_demo_test/motion_demo/CMakeLists.txt Normal file
View File

@ -0,0 +1,51 @@
cmake_minimum_required(VERSION 3.5)
project(demo_motion)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(protocol REQUIRED)
find_package(geometry_msgs REQUIRED)
add_executable(demo_motion src/demo_motion.cpp)
target_include_directories(demo_motion PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
demo_motion
"rclcpp"
"std_msgs"
protocol
)
install(TARGETS demo_motion
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

25
test/cyberdog_demo_test/motion_demo/package.xml Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>demo_motion</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="baizhongxing@xiaomi.com">axing</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>protocol</depend>
<exec_depend>geometry_msgs</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

83
test/cyberdog_demo_test/motion_demo/src/demo_motion.cpp Normal file
View File

@ -0,0 +1,83 @@
/*
需求：订阅发布方发布的消息，并在终端输出
流程：
1.包含头文件；
2.初始化ROS2客户端
3.自定义节点类
3-1 创建一个订阅方
3-2 解析并输出数据
4.调用spin函数，并传入节点对象指针
5.资源释放
*/
// 1.包含头文件；
#include <memory>
#include "rclcpp/rclcpp.hpp"
// #include "std_msgs/msg/string.hpp"
#include <protocol/msg/motion_servo_cmd.hpp>
#include <protocol/msg/motion_servo_response.hpp>
#include "std_msgs/msg/float64.hpp"
#include <chrono>
using std::placeholders::_1;
// 3.自定义节点类
class MotionSubscriberNode: public rclcpp::Node {
public:
MotionSubscriberNode()
: Node("motion_node_cpp") {
RCLCPP_INFO(this->get_logger(), "订阅方创建！");
// 创建速度发布者，连接到 "motion_servo_cmd" topic
velocity_publisher_ = create_publisher<protocol::msg::MotionServoCmd>("motion_servo_cmd", 10);
// 创建频率发布者，连接到 "velocity_frequency" topic
frequency_publisher_ = create_publisher<std_msgs::msg::Float64>("velocity_frequency", 10);
// 创建定时器，以指定频率发布速度消息
timer_ = create_wall_timer(std::chrono::milliseconds(1000), std::bind(&MotionSubscriberNode::publishVelocity, this));
}
private:
void publishVelocity() {
static auto servo_response_msg = std::make_shared<protocol::msg::MotionServoCmd>();
servo_response_msg->motion_id = 111;
// servo_response_msg->code = code;
velocity_publisher_->publish(*servo_response_msg);
// // 发布频率消息（用于演示）
// auto frequency_msg = std_msgs::msg::Float64();
// frequency_msg.data = 1.0 / 1.0; // 设置频率为1 Hz
// frequency_publisher_->publish(frequency_msg);
servo_response_msg->motion_id = 303;
servo_response_msg->vel_des = std::vector<float>{0.2, 0, 0};
servo_response_msg->step_height = std::vector<float>{0.05, 0.05};
// servo_response_msg->code = code;
velocity_publisher_->publish(*servo_response_msg);
// // // 发布频率消息（用于演示）
// auto frequency_msg_motion = std_msgs::msg::Float64();
// frequency_msg_motion.data = 1.0 / 1.0; // 设置频率为1 Hz
// frequency_publisher_->publish(frequency_msg_motion);
}
rclcpp::Publisher<protocol::msg::MotionServoCmd>::SharedPtr velocity_publisher_;
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr frequency_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char const *argv[])
{
// 2.初始化ROS2客户端
rclcpp::init(argc, argv);
auto node = std::make_shared<MotionSubscriberNode>();
// 4.调用spin函数，并传入节点对象指针
rclcpp::spin(node);
// 5.资源释放
rclcpp::shutdown();
return 0;
}

65
test/cyberdog_demo_test/test_my/CMakeLists.txt Normal file
View File

@ -0,0 +1,65 @@
cmake_minimum_required(VERSION 3.8)
project(test_my)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(protocol REQUIRED)
# add_executable(camera_subscriber_depth_node src/camera_subscriber_color_depth.cpp)
# add_executable(camera_subscriber_node src/camera_subscriber_node.cpp)
add_executable(client_node_cpp src/test_client.cpp)
# target_include_directories(camera_subscriber_depth_node PUBLIC
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>)
# target_include_directories(camera_subscriber_node PUBLIC
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>)
target_include_directories(client_node_cpp PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
# target_compile_features(camera_subscriber_depth_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
# target_compile_features(camera_subscriber_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
target_compile_features(client_node_cpp PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
# ament_target_dependencies(
# camera_subscriber_depth_node
# "rclcpp"
# "cv_bridge"
# )
# ament_target_dependencies(
# camera_subscriber_node
# "rclcpp"
# "cv_bridge"
# )
ament_target_dependencies(
client_node_cpp
"rclcpp"
"cv_bridge"
protocol
)
# install(TARGETS camera_subscriber_depth_node
# DESTINATION lib/${PROJECT_NAME})
# install(TARGETS camera_subscriber_node
# DESTINATION lib/${PROJECT_NAME})
install(TARGETS client_node_cpp
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

23
test/cyberdog_demo_test/test_my/package.xml Normal file
View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>test_my</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="baizhongxing@xiaomi.com">axing</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>cv_bridge</depend>
<depend>protocol</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>libopencv-dev</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

226
test/cyberdog_demo_test/test_my/src/test_client.cpp Normal file
View File

@ -0,0 +1,226 @@
#include "rclcpp/rclcpp.hpp"
#include "protocol/srv/camera_service.hpp"
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
using protocol::srv::CameraService;
using namespace std::chrono_literals;
// 定义节点类
class CameraClient: public rclcpp::Node {
public:
CameraClient():Node("client_node_cpp") {
client_ = this->create_client<CameraService>("camera_service");
RCLCPP_INFO(this->get_logger(), "客户端创建成功");
// // 等待服务就绪
while (!client_->wait_for_service(std::chrono::seconds(5))) {
if (!rclcpp::ok()) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "强行终止客户端！");
return;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "服务器连接中！");
}
// if (!client_->wait_for_service(std::chrono::seconds(5))) {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "camera_service is not activate");
// return;
// }
auto request = std::make_shared<CameraService::Request>();
// request->command = CameraService::Request::START_IMAGE_PUBLISH;
// request->width = 640;
// request->height = 480;
// request->fps = 30;
CameraClient::CallCamService(false, request, 640, 480, 30);
auto future_result1 = client_->async_send_request(request);
RCLCPP_INFO(this->get_logger(), "请求stop服务");
std::future_status status1 = future_result1.wait_for(std::chrono::seconds(10));
printf("status1 = %d\n", status1);
// printf("result1 = %d\n", future_result1.get()->result);
CameraClient::CallCamService(true, request, 640, 480, 30);
auto future_result = client_->async_send_request(request);
RCLCPP_INFO(this->get_logger(), "请求start服务");
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future_result) ==
rclcpp::FutureReturnCode::SUCCESS) {
auto result = future_result.get();
std::future_status status = future_result.wait_for(std::chrono::seconds(10));
if (result->result == CameraService::Response::RESULT_SUCCESS) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "contrl camera succeed 2！: ");
// 订阅topic
color_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
"image", rclcpp::SystemDefaultsQoS(),
std::bind(&CameraClient::colorCallback, this, std::placeholders::_1));
} else {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "contrl camera failed 2:");
}
} else {
RCLCPP_ERROR(this->get_logger(), "服务调用未能完成。");
}
// std::future_status status = future_result.wait_for(std::chrono::seconds(10));
// printf("status = %d\n", status);
// printf("result = %d\n", future_result.get()->result);
// if (status == std::future_status::ready) {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "ready success 1！: ");
// // uint8_t result = future_result.get()->result;
// if (future_result.get()->result == CameraService::Response::RESULT_SUCCESS) {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "contrl camera succeed 2！: ");
// // 订阅topic
// auto rgb_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
// "image", rclcpp::SystemDefaultsQoS(),
// std::bind(&CameraClient::colorCallback, this, std::placeholders::_1));
// } else {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "contrl camera failed 2:");
// }
// } else {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "ready failed 1！:");
// }
// if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::FutureReturnCode::SUCCESS) {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "响应失败！");
// } else {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "响应成功！");
// // auto result = client_->async_send_request(request);
// if (result.wait_for(std::chrono::seconds(1)) == std::future_status::ready) {
// if (result.get()->result == CameraService::Response::RESULT_SUCCESS) {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "响应成功！");
// // ros2::this_node::getNamespace();
// // 订阅topic
// auto rgb_subscriber_ = this->create_subscription<sensor_msgs::msg::Image>(
// "image", rclcpp::SystemDefaultsQoS(),
// std::bind(&CameraClient::colorCallback, this, std::placeholders::_1));
// } else {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "响应失败！");
// }
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "响应成功！");
// } else {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "响应失败！");
// }
// }
}
// 3-2.等待服务连接（对于服务通信而言，如果客户端连接不到服务器，那么不能发送请求）；
/*
连接服务器实现，如果成功返回ture,如果是失败，返回false;
*/
bool connect_service() {
// 设置指定超时时间内连接服务器，如果连接上，返回ture,否则返回false;
// client_->wait_for_service(1s);
// 循环以1s为超时时间连接服务器，直到连接到服务器才退出循环
while(!client_->wait_for_service(5s)) {
// 按下ctrlC是要结束ros2程序，意味着要释放资源，比如：关闭 context；
if (!rclcpp::ok()) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "强行终止客户端！");
return false;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "服务器连接中！");
}
return true;
}
void colorCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
// 将ROS图像消息转换为OpenCV格式
cv_bridge::CvImagePtr cv_image;
RCLCPP_INFO(this->get_logger(), "input colorCallback");
try
{
cv_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
return;
}
// 获取彩色图像
cv::Mat color_image = cv_image->image;
// 检查图像是否成功读取
if (color_image.empty()) {
std::cerr << "无法读取图像文件" << std::endl;
return;
}
// 在彩色图像上执行视觉开发操作
// ...
// 显示彩色图像
RCLCPP_INFO(this->get_logger(), "save color image");
for (int i = 0; i<1; i++)
{
bool success = cv::imwrite("/home/mi/Camera/Color_Image.png", color_image);
// 检查图像是否成功保存
if (success) {
std::cout << "图像保存成功" << std::endl;
} else {
std::cerr << "图像保存失败" << std::endl;
return;
}
}
while (!client_->wait_for_service(std::chrono::seconds(5))) {
if (!rclcpp::ok()) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "强行终止客户端！");
auto requestend = std::make_shared<CameraService::Request>();
CameraClient::CallCamService(false, requestend, 640, 480, 30);
auto future_result1 = client_->async_send_request(requestend);
RCLCPP_INFO(this->get_logger(), "请求stop服务");
return;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "服务器连接中！");
}
// cv::waitKey(1);
}
private:
rclcpp::Client<CameraService>::SharedPtr client_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr color_subscriber_;
bool CallCamService(bool activate, std::shared_ptr<protocol::srv::CameraService::Request> &request, int height, int width, int fps)
{
request->command = activate ?
protocol::srv::CameraService::Request::START_IMAGE_PUBLISH :
protocol::srv::CameraService::Request::STOP_IMAGE_PUBLISH;
if (activate) {
request->height = height;
request->width = width;
request->fps = fps;
}
return true;
}
};
int main(int argc, char ** argv)
{
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "input main");
// 2. 初始化ROS2客户端；
rclcpp::init(argc, argv);
// 4 创建客户端对象(自定义节点类型的)
auto client_node = std::make_shared<CameraClient>();
// // 调用客户端对象的连接服务器功能；
// bool flag = client->connect_service();
// // 根据连接结果做进一步处理
// if (!flag) {
// /*
// rclcpp::get_logger("name")创建不依赖context
// */
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "服务器连接失败，程序推出");
// return 0;
// }
rclcpp::spin(client_node);
// 5. 释放资源
rclcpp::shutdown();
return 0;
}

50
test/cyberdog_demo_test/tof_demo/CMakeLists.txt Normal file
View File

@ -0,0 +1,50 @@
cmake_minimum_required(VERSION 3.5)
project(demo_tof)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(protocol REQUIRED)
add_executable(demo_tof src/demo_tof.cpp)
target_include_directories(demo_tof PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
demo_tof
"rclcpp"
"std_msgs"
protocol
)
install(TARGETS demo_tof
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

22
test/cyberdog_demo_test/tof_demo/package.xml Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>demo_tof</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="baizhongxing@xiaomi.com">axing</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>protocol</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

58
test/cyberdog_demo_test/tof_demo/src/demo_tof.cpp Normal file
View File

@ -0,0 +1,58 @@
/*
需求：订阅发布方发布的消息，并在终端输出
流程：
1.包含头文件；
2.初始化ROS2客户端
3.自定义节点类
3-1 创建一个订阅方
3-2 解析并输出数据
4.调用spin函数，并传入节点对象指针
5.资源释放
*/
// 1.包含头文件；
#include <memory>
#include "rclcpp/rclcpp.hpp"
// #include "std_msgs/msg/string.hpp"
#include <protocol/msg/head_tof_payload.hpp>
using std::placeholders::_1;
// 3.自定义节点类
class TofSubscriberNode: public rclcpp::Node {
public:
TofSubscriberNode()
: Node("tof_node_cpp") {
RCLCPP_INFO(this->get_logger(), "订阅方创建！");
// 3-1 创建一个订阅方
/*
模板：对象的消息类型
参数：
1.话题名称
2.QOS:服务质量管理，对类长度
3.回调函数。
返回值：订阅对象的指针
*/
subscription_ = this->create_subscription<protocol::msg::HeadTofPayload>(
"head_tof_payload", 10, std::bind(&TofSubscriberNode::do_callback, this, _1));
}
private:
void do_callback(const protocol::msg::HeadTofPayload::SharedPtr msg) const {
// 3-2 解析并输出数据
RCLCPP_INFO(this->get_logger(), "订阅到的消息: left_head.data[6] = %f", msg->left_head.data[6]);
RCLCPP_INFO(this->get_logger(), "订阅到的消息: msg->right_head.data[6] = %f", msg->right_head.data[6]);
}
rclcpp::Subscription<protocol::msg::HeadTofPayload>::SharedPtr subscription_;
};
int main(int argc, char const *argv[])
{
// 2.初始化ROS2客户端
rclcpp::init(argc, argv);
// 4.调用spin函数，并传入节点对象指针
rclcpp::spin(std::make_shared<TofSubscriberNode>());
// 5.资源释放
rclcpp::shutdown();
return 0;
}