ros2中定义接口类型如何定义Point2D类型的request

时间: 2025-07-06 21:50:49 AIGC 浏览: 20
### 定义带有 Point2D 类型的 Request 消息接口 在 ROS 2 中创建包含 `Point2D` 类型的消息接口涉及几个步骤。首先,需要理解 ROS 2 提供的标准消息包以及如何扩展这些标准来满足特定需求。 #### 使用几何消息中的 Point 类型 ROS 2 几何相关消息包提供了多种用于表示空间位置的数据结构[^1]。对于二维平面上的位置表达,可以考虑使用来自 `geometry_msgs/msg/Point` 或者更具体的 `geometry_msgs/msg/Point2D`(如果存在)。然而,在某些情况下,特别是当官方库未提供直接支持时,则需自行定义所需类型。 #### 创建自定义消息文件 为了引入新的消息类型到项目里,应该在一个名为 `msg` 的子目录下新建 `.msg` 文件,并在此处声明新字段: ```plaintext # my_package/msg/MyCustomRequest.msg float64 x float64 y ``` 上述代码片段展示了最基础的方式去模拟一个 `Point2D` 结构体;当然也可以通过继承已有的几何消息类来进行更加复杂的定制化工作。 #### 注册并编译自定义消息 完成 `.msg` 文件编写之后,还需要修改 CMakeLists.txt 和 package.xml 来确保构建工具能够识别新增加的内容。具体来说就是添加依赖项并且调用相应的宏指令让 ament_cmake 处理它们: ```cmake find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/MyCustomRequest.msg" ) ``` 同时更新 `package.xml` 添加如下行以指明所使用的接口生成器插件: ```xml <buildtool_depend>rosidl_default_generators</buildtool_depend> <exec_depend>rosidl_default_runtime</exec_depend> ``` 最后执行 colcon build 构建整个工程即可使自定义的消息生效。 #### Python客户端实例 一旦完成了以上准备工作,就可以利用 RCLPY 库轻松地发送含有 `Point2D` 成员变量的服务请求了: ```python import rclpy from my_package.srv import MyCustomService from geometry_msgs.msg import Point, Point2D def main(args=None): rclpy.init(args=args) node = rclpy.create_node('my_custom_client') client = node.create_client(MyCustomService, 'service_name') while not client.wait_for_service(timeout_sec=1.0): node.get_logger().info('等待服务启动...') request = MyCustomService.Request() point_2d = Point2D() if hasattr(Point2D(), '__init__') else Point(x=1.0, y=2.0) # 如果没有Point2D则退回到Point setattr(request, 'point_field', point_2d) future = client.call_async(request) try: response = future.result() node.get_logger().info(f'收到响应: {response}') except Exception as e: node.get_logger().error(f'Service call failed %r' % (e,)) finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main() ``` 此脚本展示了一个简单的例子,其中包含了向服务器发出携带 `Point2D` 数据作为参数之一的服务调用过程。
阅读全文

相关推荐

#include "RelativeMove.h" namespace rei_relative_move { RelativeMove::RelativeMove() : motionModel_(0), getPoseReady_(false), moveFinished_(true), robotModel_(0) {} RelativeMove::~RelativeMove() { if (listenTfThread_.joinable()) { listenTfThread_.join(); } geometry_msgs::Twist stopCmd; velPub_.publish(stopCmd); } int8_t RelativeMove::Init(ros::NodeHandle& nh) { tfBuffer_ = std::make_unique<tf2_ros::Buffer>(); tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_); xPid_ = std::make_shared<rei_tools::ReiPID>(1.0, 0, 0.0); yPid_ = std::make_shared<rei_tools::ReiPID>(1.0, 0, 0.0); thetaPid_ = std::make_shared<rei_tools::ReiPID>(2.0, 0, 0.0); xPid_->setOutputLimit(0.5, 0.05); yPid_->setOutputLimit(0.5, 0.05); thetaPid_->setOutputLimit(1.0, 0.2); relativeMoveServer_ = nh_.advertiseService( "relative_move", &RelativeMove::RelativeMoveCallback, this); velPub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 5); xErrPub_ = nh_.advertise<std_msgs::Float64>("relative/x_err", 5); yErrPub_ = nh_.advertise<std_msgs::Float64>("relative/y_err", 5); thetaErrPub_ = nh_.advertise<std_msgs::Float64>("relative/theta_err", 5); expectXErr_ = 0.01; expectYErr_ = 0.01; expectThetaErr_ = 0.01; return 0; } void RelativeMove::SetXPid(int p, int i, int d) { xPid_->setP(p); xPid_->setI(i); xPid_->setD(d); } void RelativeMove::SetYPid(int p, int i, int d) { yPid_->setP(p); yPid_->setI(i); yPid_->setD(d); } void RelativeMove::SetThetaPid(int p, int i, int d) { thetaPid_->setP(p); thetaPid_->setI(i); thetaPid_->setD(d); } void RelativeMove::ListenTf(std::string frameId, std::string childFrameId) { geometry_msgs::TransformStamped transform; std::string errMsg; if (!tfBuffer_->canTransform(frameId, childFrameId, ros::Time(0), ros::Duration(2.0), &errMsg)) { ROS_ERROR("Unable to get pose from TF: %s", errMsg.c_str()); return; } { std::unique_lock<std::mutex> lock(getFlagMutex_); getPoseReady_ = true; } ros::Rate rate(10.0); while (ros::ok() && (!GetFinishFlag())) { try { transform = tfBuffer_->lookupTransform(frameId, childFrameId, ros::Time(0)); SetTfPose(transform.transform); } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); continue; } rate.sleep(); } } void RelativeMove::SetTfPose(geometry_msgs::Transform& tfPose) { std::unique_lock<std::mutex> lock(getTfPoseMutex_); tfPose_ = tfPose; } void RelativeMove::GetTfPose(geometry_msgs::Transform& trans) { std::unique_lock<std::mutex> lock(getTfPoseMutex_); trans = tfPose_; } geometry_msgs::Pose2D RelativeMove::GetTargetGoal( const geometry_msgs::Pose2D& goal) { tf2::Quaternion goal_quat; goal_quat.setRPY(0, 0, goal.theta); geometry_msgs::Transform robotPose; GetTfPose(robotPose); tf2::Transform robotPoseTrans, goalPoseTrans, goalBaseRobotTrans; goalBaseRobotTrans.setOrigin(tf2::Vector3(goal.x, goal.y, 0)); goalBaseRobotTrans.setRotation(goal_quat); geometry_msgs::Transform goalPose; tf2::fromMsg(robotPose, robotPoseTrans); goalPoseTrans = robotPoseTrans * goalBaseRobotTrans; // ROS_INFO("robotPose: %lf, %lf, %lf, %lf", robotPose.translation.x, // robotPose.translation.y, robotPose.rotation.z, // robotPose.rotation.w); // ROS_INFO("goalBaseRobotTrans: %lf, %lf, %lf, %lf", // goalBaseRobotTrans.getOrigin().x(), // goalBaseRobotTrans.getOrigin().y(), // goalBaseRobotTrans.getRotation().z(), // goalBaseRobotTrans.getRotation().w()); // ROS_INFO("goalPoseTrans: %lf, %lf, %lf, %lf", // goalPoseTrans.getOrigin().x(), // goalPoseTrans.getOrigin().y(), // goalPoseTrans.getRotation().z(), // goalPoseTrans.getRotation().w()); geometry_msgs::Pose2D goalReuslt; goalReuslt.x = goalPoseTrans.getOrigin().getX(); goalReuslt.y = goalPoseTrans.getOrigin().getY(); tf2::Matrix3x3 mat(goalPoseTrans.getRotation()); double roll, pitch; mat.getRPY(roll, pitch, goalReuslt.theta); return goalReuslt; } int8_t RelativeMove::GetBaseToGoal(std::string frameId, const geometry_msgs::Pose2D& inGoal) { std::string errMsg; if (!tfBuffer_->canTransform(frameId, "base_link", ros::Time(0), ros::Duration(2.0), &errMsg)) { ROS_ERROR("Unable to get pose from TF: %s", errMsg.c_str()); return -1; } try { geometry_msgs::TransformStamped transform = tfBuffer_->lookupTransform(frameId, "base_link", ros::Time(0)); SetTfPose(transform.transform); } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); return -1; } baseToTargetPose_ = GetTargetGoal(inGoal); return 0; } int8_t RelativeMove::MovXY(double x, double y) { xPid_->reset_integral(); yPid_->reset_integral(); geometry_msgs::Pose2D goalReal; geometry_msgs::Pose2D targetGoal; targetGoal.x = x; targetGoal.y = y; double xErr, yErr; ros::Rate loop(10); while (ros::ok()) { goalReal = GetTargetGoal(targetGoal); // ROS_WARN("goalReal: %lf, %lf", goalReal.x, goalReal.y); xErr = .0; yErr = .0; if (x != 0) xErr = goalReal.x; if (y != 0) yErr = goalReal.y; std_msgs::Float64 errMsg; errMsg.data = xErr; xErrPub_.publish(errMsg); errMsg.data = yErr; yErrPub_.publish(errMsg); // ROS_WARN("err: %lf, %lf", xErr, yErr); if ((fabs(xErr) < expectXErr_) && (fabs(yErr) < expectYErr_)) { velPub_.publish(stopCmd_); break; } else { geometry_msgs::Twist velCmd; if (fabs(xErr) > expectXErr_) velCmd.linear.x = xPid_->compute(0.0, xErr); if (fabs(yErr) > expectYErr_) velCmd.linear.y = yPid_->compute(0.0, yErr); velPub_.publish(velCmd); } loop.sleep(); } return 0; } int8_t RelativeMove::MovTheta(double theta) { thetaPid_->reset_integral(); geometry_msgs::Pose2D goalReal; geometry_msgs::Pose2D targetGoal; targetGoal.theta = theta; double thetaErr; ros::Rate loop(10); while (ros::ok()) { goalReal = GetTargetGoal(targetGoal); // ROS_WARN("goalReal: %lf, %lf", goalReal.x, goalReal.y); thetaErr = goalReal.theta; // ROS_WARN("thetaErr: %lf",thetaErr); if (fabs(thetaErr) < expectThetaErr_) { velPub_.publish(stopCmd_); break; } else { geometry_msgs::Twist velCmd; velCmd.angular.z = thetaPid_->compute(0.0, thetaErr); velPub_.publish(velCmd); } loop.sleep(); } return 0; } bool RelativeMove::RelativeMoveCallback( relative_move::SetRelativeMove::Request& req, relative_move::SetRelativeMove::Response& res) { if (listenTfThread_.joinable()) { res.message = "last move task still run"; return false; } getPoseReady_ = false; moveFinished_ = false; res.message = "Get tf trans error"; if (GetBaseToGoal(req.global_frame, req.goal) < 0) return true; listenTfThread_ = std::thread(&RelativeMove::ListenTf, this, "base_link", req.global_frame); double lastTime = ros::Time::now().toSec(); ros::Rate loop(10); while (ros::ok() && (!GetReadyFlag())) { if ((ros::Time::now().toSec() - lastTime) > 2) return true; loop.sleep(); } if (robotModel_) { MovXY(baseToTargetPose_.x, baseToTargetPose_.y); if (req.goal.theta != 0) MovTheta(baseToTargetPose_.theta); } else { geometry_msgs::Pose2D tmpPose; tmpPose = GetTargetGoal(req.goal); double tmpTheta = atan2(tmpPose.y, tmpPose.x); if(tmpTheta>3.1415927) tmpTheta = tmpTheta - M_PI; else if(tmpTheta<-3.1415927) tmpTheta = tmpTheta + M_PI; MovTheta(tmpTheta); MovXY(baseToTargetPose_.x, 0.0); MovTheta(baseToTargetPose_.theta); } SetFinishFlag(true); if (listenTfThread_.joinable()) listenTfThread_.join(); res.message = ""; res.success = true; return true; } } // namespace rei_relative_move翻译该代码并解释用处

构建一个ROS2节点,需要实现以下功能:1.将LaserScan数据转换为PointCloud2(将每个激光点的距离和角度转换为笛卡尔坐标。)并发布到新话题。2.订阅该点云话题并在RViz中显示(这一步主要是用户操作,但我们需要确保发布点云话题)。3.订阅/fusion_pose话题(geometry_msgs/msg/PoseStamped类型)以获取实时位姿,并保存最新的位姿数据(用一个成员变量存储,并注意线程安全,因为回调是并发的)。4.,当收到上位机开始检测的命令时服务被调用,先回复请求已收到,然后执行货架检测算法计算货架中心位姿(需要用到最近的点云),将结果(geometry_msgs/msg/PoseStamped[2] result发给规划节点,同时发布以便在RViz中可视化。请给出详细c++代码实现与注释,可参考如下步骤-创建一个类,比如叫做ShelfDetectionNode,继承自rclpy.node.Node。-在初始化中:a.创建一个订阅者订阅/scan话题(类型sensor_msgs.msg.LaserScan),设置回调函数scan_callback。b.创建一个发布者,发布点云到/laser_pointcloud2。c.创建一个订阅者订阅/fusion_pose(类型geometry_msgs.msg.PoseStamped),回调函数pose_callback更新最新的位姿。d.创建一个服务,用于触发货架检测(服务类型需要自定义,比如example_interfaces/srv/Trigger,或者自定义一个空的服务,因为只需要触发,不需要参数。但响应部分需要先回复,然后异步处理,所以服务类型应该有一个空请求和一个立即回复的响应,然后节点再异步处理并发布结果。e.创建发布者,用于发布检测到的货架中心位姿(类型geometry_msgs.msg.PoseStamped)。注意:由于服务回调中需要执行耗时操作(货架检测),为了避免阻塞服务回调(进而阻塞ROS2的executor),我们可以在服务回调中启动一个线程来处理检测任务,或者使用异步方式。但是,ROS2的回调默认是单线程的(除非使用MultiThreadedExecutor),所以我们可以将检测任务放到另一个线程中。另一种方法是:在服务回调中,先发送响应,然后将检测任务放入一个异步任务(比如通过一个Future)或者使用一个定时器来触发后续处理。这里我们采用先发送响应,然后利用节点的executor来调度检测任务。但是,由于检测任务可能耗时,我们最好使用单独的线程处理,避免阻塞其他回调。因此,我们可以在服务回调中:-立即发送响应(表示已收到请求)。-然后,将检测任务放入一个线程池或启动一个新线程来执行检测(注意:访问共享数据如最新位姿和点云时需要加锁)。由于我们只需要存储最新的位姿和最新的点云(只保存最新的点云),所以我们可以:-在scan_callback中保存最新的点云(同样用一个成员变量存储,并加锁保护)。-在pose_callback中保存最新的位姿(加锁保护)。然后,在检测线程中,我们获取这两个数据(注意:获取时加锁,并且尽量缩短加锁时间)进行货架检测。货架检测算法用一个函数detect_shelf来表示,该函数输入是当前点云,输出是货架中心的位姿(一个PoseStamped)。最后,发布这个位姿在RViz中显示货架中心位置。

#include <ros/ros.h> #include <image_transport/image_transport.h> #include <cv_bridge/cv_bridge.h> #include <sensor_msgs/image_encodings.h> #include <opencv2/imgproc/imgproc.hpp> #include <opencv2/highgui/highgui.hpp> #include <stdio.h> #include <stdlib.h> #include "opencv2/opencv.hpp" #include <moveit/move_group_interface/move_group_interface.h> #include <tf/transform_listener.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/robot_trajectory/robot_trajectory.h> #include "myself_pkg/uart.h" #include <sys/stat.h> #include <cmath> #include <xarm_driver.h> #include <thread> // 引入线程库 #include <atomic> static const std::string OPENCV_WINDOW = "Image window"; #define M_PI 3.14159265358979323846 tf::Vector3 obj_camera_frame1, obj_robot_frame; int flag_start=1; ros::Subscriber rgb_sub; // 相机内参 double fx = 953.4568; // x轴方向的焦距 double fy = 949.837; // y轴方向的焦距 double cx = 658.66659; // x轴方向的光学中心 double cy = 366.82704; // y轴方向的光学中心 // 物体高度 double objectHeight = 0.34432666; // 假设物体的高度为1米 double k1 = 0.133507; // double k2 =-0.213178; // double p1 = 0.006242; // double p2 = 0.005494; // /* double fx = 1084.54479; // x轴方向的焦距 double fy = 950.11576; // y轴方向的焦距 double cx = 642.85519; // x轴方向的光学中心 double cy = 354.52482; // y轴方向的光学中心 // 物体高度 double objectHeight = 0.34432666; // 假设物体的高度为1米 double k1 = 0.141430; // double k2 =-0.384089; // double p1 = 0.003167; // double p2 = 0.002440; // */ cv::Mat K = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); // 构造畸变参数向量 cv::Mat distCoeffs = (cv::Mat_<double>(5, 1) << k1, k2, p1, p2, 0); // 待校正的像素坐标 cv::Point2d pixelPoint(640, 360); // 假设像素坐标为(320, 240) cv::Mat cameraPointMat = (cv::Mat_<double>(3, 1) << 0, 0,0); /** // 定义一个结构体表示四元数,用于三维空间中的旋转表示 struct Quaternion { double w; // 四元数的实部 double x; // 四元数的虚部 x double y; // 四元数的虚部 y double z; // 四元数的虚部 z }; Quaternion eulerToQuaternion(double roll, double pitch, double yaw) { // 计算半角 double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); double sp = sin(pitch * 0.5); double cr = cos(roll * 0.5); double sr = sin(roll * 0.5); Quaternion q; // 根据欧拉角到四元数的转换公式计算四元数的实部 q.w = cr * cp * cy + sr * sp * sy; // 根据欧拉角到四元数的转换公式计算四元数的虚部 x q.x = sr * cp * cy - cr * sp * sy; // 根据欧拉角到四元数的转换公式计算四元数的虚部 y q.y = cr * sp * cy + sr * cp * sy; // 根据欧拉角到四元数的转换公式计算四元数的虚部 z q.z = cr * cp * sy - sr * sp * cy; return q; } */ int move_lineb_test(xarm_msgs::Move srv, ros::ServiceClient client, float x_mm0, float y_mm0, float z_mm0, double roll0, double pitch0, double yaw0, float x_mm1, float y_mm1, float z_mm1, double roll1, double pitch1, double yaw1, float x_mm2, float y_mm2, float z_mm2, double roll2, double pitch2, double yaw2, float x_mm3, float y_mm3, float z_mm3, double roll3, double pitch3, double yaw3, float x_mm4, float y_mm4, float z_mm4, double roll4, double pitch4, double yaw4); void control_suction_during_move(float x_mm4, float y_mm4, float z_mm4); // 异步吸盘控制 auto suction_control = [](int speed){ std::thread([speed](){ for(int i=0; i<1; i++){ writeSpeed(speed); std::this_thread::sleep_for(std::chrono::milliseconds(800)); } }).detach(); // 分离线程 }; class XArmAPItest { ros::NodeHandle nh_; image_transport::ImageTransport it_; image_transport::Subscriber image_sub_; tf::TransformListener listener_; tf::StampedTransform camera_to_robot_; public: XArmAPItest() : it_(nh_) { moveit::planning_interface::MoveGroupInterface arm("xarm7"); sleep(0.5); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; sleep(0.5); //异步任务处理器,防阻塞 ros::AsyncSpinner spinner(1); spinner.start(); // 创建一个新的障碍物消息 moveit_msgs::CollisionObject collision_object; collision_object.header.frame_id = "world"; // 设置障碍物的参考坐标系,通常为世界坐标系 // 设置障碍物的 ID collision_object.id = "table"; // 定义障碍物的形状和尺寸 shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = 2.0; // 长 primitive.dimensions[1] = 2.0; // 宽 primitive.dimensions[2] = 0.1; // 高 // 定义障碍物的姿态 geometry_msgs::Pose obstacle_pose; obstacle_pose.orientation.w = 1.0; // 默认姿态为单位四元数 obstacle_pose.position.x = 0.0; // x 位置 obstacle_pose.position.y = 0.0; // y 位置 obstacle_pose.position.z = -0.06; // z 位置 // 将障碍物的形状和姿态添加到障碍物消息中 collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(obstacle_pose); // 设置操作类型为添加障碍物 collision_object.operation = collision_object.ADD; // 发送障碍物消息到规划场景 moveit_msgs::PlanningScene planning_scene; planning_scene.world.collision_objects.push_back(collision_object); planning_scene.is_diff = true; planning_scene_interface.applyPlanningScene(planning_scene); // 应用障碍物到规划场景 ROS_INFO("Obstacle added"); /** //回初位置 arm.setNamedTarget("home");//设置目标 arm.move();//执行 sleep(0.5); double targetPose[7] = {-0.166690,0.00000, -0.076904,1.173601, 0.015010,1.21220,-0.260379}; std::vector<double> joint_group_positions(7); joint_group_positions[0] = targetPose[0]; joint_group_positions[1] = targetPose[1]; joint_group_positions[2] = targetPose[2]; joint_group_positions[3] = targetPose[3]; joint_group_positions[4] = targetPose[4]; joint_group_positions[5] = targetPose[5]; joint_group_positions[6] = targetPose[6]; arm.setJointValueTarget(joint_group_positions); arm.move(); sleep(0.5); */ try { listener_.waitForTransform("link_base", "camera_link", ros::Time(0), ros::Duration(50.0)); } catch (tf::TransformException &ex) { ROS_ERROR("[adventure_tf]: (wait) %s", ex.what()); ros::Duration(1.0).sleep(); } try { listener_.lookupTransform("link_base", "camera_link", ros::Time(0), camera_to_robot_); tf::Vector3 translation = camera_to_robot_.getOrigin(); objectHeight=translation.getZ(); std::cout << objectHeight << std::endl; } catch (tf::TransformException &ex) { ROS_ERROR("[adventure_tf]: (lookup) %s", ex.what()); } // 订阅相机图像 image_sub_ = it_.subscribe("/camera/color/image_raw", 1, &XArmAPItest::Cam_RGB_Callback, this); sleep(1); } /** // 封装四元数转欧拉角的函数 std::tuple<double, double, double> quaternionToEuler(const geometry_msgs::Quaternion& q) { double x = q.x; double y = q.y; double z = q.z; double w = q.w; // 计算绕 x 轴旋转的弧度(roll) double sinr_cosp = 2 * (w * x + y * z); double cosr_cosp = 1 - 2 * (x * x + y * y); double roll = std::atan2(sinr_cosp, cosr_cosp); // 计算绕 y 轴旋转的弧度(pitch) double sinp = 2 * (w * y - z * x); double pitch; if (std::abs(sinp) >= 1) pitch = std::copysign(M_PI / 2, sinp); // 使用 90 度避免数值问题 else pitch = std::asin(sinp); // 计算绕 z 轴旋转的弧度(yaw) double siny_cosp = 2 * (w * z + x * y); double cosy_cosp = 1 - 2 * (y * y + z * z); double yaw = std::atan2(siny_cosp, cosy_cosp); return std::make_tuple(roll, pitch, yaw); } */ void Grasping(double a, double b, double z, double Angle) { std::cout << "Grasping" << std::endl; // 输出提示信息 moveit::planning_interface::MoveGroupInterface arm("xarm7"); ros::AsyncSpinner spinner(1); spinner.start(); std::string end_effector_link = arm.getEndEffectorLink(); std::string reference_frame = "link_base"; arm.setPoseReferenceFrame(reference_frame); // 声明一个变量用于存储机械臂当前的位姿信息 geometry_msgs::PoseStamped homePose; // 获取机械臂末端执行器当前的位姿并赋值给 homePose 变量 homePose = arm.getCurrentPose(); sleep(1); writeSpeed(1); writeSpeed(1); writeSpeed(1); sleep(0.8); /*******************第1次抓取*********************/ /*** if(flag_start==1) { //移动到抓取方块上方 flag_start=2; //将初始位姿加入路点列表 //waypoints.push_back(target_pose); geometry_msgs::Pose target_pose; target_pose.position.x = x; target_pose.position.y = y; target_pose.position.z = 0; double roll = 180*M_PI/180; // 绕 x 轴旋转的弧度 double pitch = 0; // 绕 y 轴旋转的弧度 double yaw = 0; // 绕 z 轴旋转的弧度 // 将目标姿态的位置坐标从米转换为毫米 float x_mm = static_cast<float>(target_pose.position.x * 1000); float y_mm = static_cast<float>(target_pose.position.y * 1000); float z_mm = static_cast<float>(target_pose.position.z * 1000); } */ double x = a + 0.0001-0.0041; double y = b + 0.0009+0.0082+0.001; /*******************抓取*********************/ if(flag_start>=1&&flag_start<35) { //抓取物块 geometry_msgs::Pose target_pose0; target_pose0 = arm.getCurrentPose(end_effector_link).pose; target_pose0.position.z = 0+0.14+0.013; target_pose0.position.x = x; target_pose0.position.y = y; double roll0 = 180*M_PI/180; // 绕 x 轴旋转的弧度 double pitch0 =0*M_PI/180; // 绕 y 轴旋转的弧度 double yaw0 = 0; // 绕 z 轴旋转的弧度 // 将目标姿态的位置坐标从米转换为毫米 float x_mm0 = static_cast<float>(target_pose0.position.x * 1000); float y_mm0 = static_cast<float>(target_pose0.position.y * 1000); float z_mm0 = static_cast<float>(target_pose0.position.z * 1000); ROS_INFO("zuobiao0: %f, %f, %f", x_mm0, y_mm0, z_mm0); // 定位抓取物块 geometry_msgs::Pose target_pose1; target_pose1 = arm.getCurrentPose(end_effector_link).pose; target_pose1.position.x = x; target_pose1.position.y = y; target_pose1.position.z = 0.09+0.0003+0.01246-0.01; double roll1 = 180*M_PI/180; // 绕 x 轴旋转的弧度 double pitch1 =0*M_PI/180; // 绕 y 轴旋转的弧度 double yaw1 = 0; // 绕 z 轴旋转的弧度 // 将目标姿态的位置坐标从米转换为毫米 float x_mm1 = static_cast<float>(target_pose1.position.x * 1000); float y_mm1 = static_cast<float>(target_pose1.position.y * 1000); float z_mm1 = static_cast<float>(target_pose1.position.z * 1000); ROS_INFO("zuobiao1: %f, %f, %f", x_mm1, y_mm1, z_mm1); //抓取物块 geometry_msgs::Pose target_pose2; target_pose2 = arm.getCurrentPose(end_effector_link).pose; target_pose2.position.z = 0+0.14+0.013; target_pose2.position.x = x; target_pose2.position.y = y; double roll2 = 180*M_PI/180; // 绕 x 轴旋转的弧度 double pitch2 =0*M_PI/180; // 绕 y 轴旋转的弧度 double yaw2 = 0; // 绕 z 轴旋转的弧度 // 将目标姿态的位置坐标从米转换为毫米 float x_mm2 = static_cast<float>(target_pose2.position.x * 1000); float y_mm2 = static_cast<float>(target_pose2.position.y * 1000); float z_mm2 = static_cast<float>(target_pose2.position.z * 1000); ROS_INFO("zuobiao2: %f, %f, %f", x_mm2, y_mm2, z_mm2); geometry_msgs::Pose target_pose3; target_pose3 = arm.getCurrentPose(end_effector_link).pose; //放置物块 target_pose3.position.z= 0.14+0.013; double roll3 = 180*M_PI/180; // 绕 x 轴旋转的弧度 double pitch3 =0*M_PI/180; // 绕 y 轴旋转的弧度 double yaw3 = Angle*M_PI/180; // 绕 z 轴旋转的弧度 /**********************************************zi色*************************************/ if(flag_start==1){ target_pose3.position.x = 0.38578-0.023+0.002; target_pose3.position.y = 0.217467-0.052;} if(flag_start==2){ target_pose3.position.x = 0.38578+0.06-0.023; target_pose3.position.y = 0.217467+0.02-0.052;} if(flag_start==3){ target_pose3.position.x = 0.38578+0.08-0.023; target_pose3.position.y = 0.217467-0.052;} /**********************************************橙色*************************************/ if(flag_start==4){ target_pose3.position.x = 0.38578+0.01-0.023; target_pose3.position.y = 0.217467+0.052-0.055;} if(flag_start==5){ target_pose3.position.x = 0.38578+0.05-0.023; target_pose3.position.y = 0.217467+0.052-0.055;} if(flag_start==6){ target_pose3.position.x = 0.38578+0.09-0.023; target_pose3.position.y = 0.217467+0.052-0.055;} /*********************************************hong色 *****************************************/ if(flag_start==7){ target_pose3.position.x = 0.38578-0.023; target_pose3.position.y = 0.217467+0.11-0.055+0.002;} if(flag_start==8){ target_pose3.position.x = 0.38578+0.02-0.023-0.002; target_pose3.position.y = 0.217467+0.11-0.055+0.002;} if(flag_start==9){ target_pose3.position.x = 0.38578+0.04-0.023-0.002; target_pose3.position.y = 0.217467+0.11-0.055;} /********************************************************huang色*****************************/ if(flag_start==10){ target_pose3.position.x = 0.38578-0.023+0.002; target_pose3.position.y = 0.217467+0.18+-0.05-0.001;} if(flag_start==11){ target_pose3.position.x = 0.38578+0.06-0.023-0.002; target_pose3.position.y = 0.217467+0.16-0.05;} if(flag_start==12){ target_pose3.position.x = 0.38578+0.08-0.023; target_pose3.position.y = 0.217467+0.18-0.05;} if(flag_start==13){ target_pose3.position.x = 0.38578+0.14-0.023-0.001; target_pose3.position.y = 0.217467+0.16-0.05;} /*****************************************************hei色*********************************** */ if(flag_start==14){ target_pose3.position.x = 0.38578+0.06-0.023; target_pose3.position.y = 0.217467+0.10-0.05;} if(flag_start==15){ target_pose3.position.x = 0.38578+0.08-0.023; target_pose3.position.y = 0.217467+0.14-0.05-0.001;} /*****************************************************zi色*************************** */ if(flag_start==16){ target_pose3.position.x = 0.38578+0.12-0.023-0.001; target_pose3.position.y = 0.217467+0.08-0.0575;} /***************************************************lan色******************************* */ if(flag_start==17){ target_pose3.position.x = 0.38578+0.11-0.023+0.001; target_pose3.position.y = 0.217467+0.12-0.05-0.001; roll3 = 180*M_PI/180; // 绕 x 轴旋转的弧度 pitch3 =0*M_PI/180; // 绕 y 轴旋转的弧度 yaw3 = Angle*M_PI/180 + M_PI/2; // 绕 z 轴旋转的弧度 } if(flag_start==18){ target_pose3.position.x = 0.38578+0.11+0.03-0.02; target_pose3.position.y = 0.217467+0.09-0.05;} if(flag_start==19){ target_pose3.position.x = 0.38578+0.11+0.03+0.04-0.02-0.002; target_pose3.position.y = 0.217467+0.09-0.05+0.001;} if(flag_start==20){ target_pose3.position.x = 0.38578+0.11+0.03+0.04+0.02-0.02-0.02-0.07; target_pose3.position.y = 0.217467+0.09+0.02-0.05+0.002;} /***************************************************ceng色*************************** */ if(flag_start==21){ target_pose3.position.x = 0.38578+0.15-0.02-0.002; target_pose3.position.y = 0.217467+0.13-0.045-0.002;} /*****************************************************lv色**************************************** */ if(flag_start==22){ target_pose3.position.x = 0.38578+0.12-0.02-0.004; target_pose3.position.y = 0.217467+0.03-0.05;} if(flag_start==23){ target_pose3.position.x = 0.38578+0.16-0.02-0.004-0.0005; target_pose3.position.y = 0.217467+0.03-0.05;} if(flag_start==24){ target_pose3.position.x = 0.38578+0.20-0.02-0.004-0.0005; target_pose3.position.y = 0.217467+0.03-0.05;} if(flag_start==25){ target_pose3.position.x = 0.38578+0.17-0.02-0.004-0.0001; target_pose3.position.y = 0.217467+0.11+0.06-0.05-0.005; roll3 = 180*M_PI/180; // 绕 x 轴旋转的弧度 pitch3 =0*M_PI/180; // 绕 y 轴旋转的弧度 yaw3 = Angle*M_PI/180 - M_PI/2; // 绕 z 轴旋转的弧度 } /***********************************************************huang色************************************** */ if(flag_start==26){ target_pose3.position.x = 0.38578+0.20-0.022; target_pose3.position.y = 0.217467+0.11+0.08-0.05-0.004-0.004;} /***********************************************************hong色************************************** */ if(flag_start==27){ target_pose3.position.x = 0.38578+0.17-0.021; target_pose3.position.y = 0.217467-0.05; roll3 = 180*M_PI/180; // 绕 x 轴旋转的弧度 pitch3 =0*M_PI/180; // 绕 y 轴旋转的弧度 yaw3 = Angle*M_PI/180 - M_PI/2; // 绕 z 轴旋转的弧度 } if(flag_start==28){ target_pose3.position.x = 0.38578+0.17-0.021; target_pose3.position.y = 0.217467+0.06-0.05; roll3 = 180*M_PI/180; // 绕 x 轴旋转的弧度 pitch3 =0*M_PI/180; // 绕 y 轴旋转的弧度 yaw3 = Angle*M_PI/180 - M_PI/2; // 绕 z 轴旋转的弧度 } /***************************************************************hei色************************************* */ if(flag_start==29){ target_pose3.position.x = 0.38578+0.22-0.025+0.002; target_pose3.position.y = 0.217467+0.10+0.04-0.05;} if(flag_start==30){ target_pose3.position.x = 0.38578+0.24-0.025; target_pose3.position.y = 0.217467+0.18-0.05;} /*************************************************ceng色*************************** */ if(flag_start==31){ target_pose3.position.x = 0.38578+0.23-0.02-0.002-0.0002; target_pose3.position.y = 0.217467+0.07-0.045-0.002-0.002;} /*************************************************lan色*************************** */ if(flag_start==32){ target_pose3.position.x = 0.38578+0.23-0.024; target_pose3.position.y = 0.217467+0.02-0.051-0.0001; roll3 = 180*M_PI/180; // 绕 x 轴旋转的弧度 pitch3 =0*M_PI/180; // 绕 y 轴旋转的弧度 yaw3 = Angle*M_PI/180 + M_PI/2; // 绕 z 轴旋转的弧度 } /*************************************************zi色*************************** */ if(flag_start==33){ target_pose3.position.x = 0.38578+0.26-0.024; target_pose3.position.y = 0.217467-0.051+0.001;} /*************************************************lv色*************************** */ if(flag_start==34){ target_pose3.position.x = 0.38578+0.25-0.024; target_pose3.position.y = 0.217467+0.10-0.051+0.001; target_pose3.position.z= 0.14+0.013+0.002; roll3 = 180*M_PI/180; // 绕 x 轴旋转的弧度 pitch3 =0*M_PI/180; // 绕 y 轴旋转的弧度 yaw3 = Angle*M_PI/180 - M_PI/2; // 绕 z 轴旋转的弧度 } // 将目标姿态的位置坐标从米转换为毫米 float x_mm3 = static_cast<float>(target_pose3.position.x * 1000); float y_mm3 = static_cast<float>(target_pose3.position.y * 1000); float z_mm3 = static_cast<float>(target_pose3.position.z * 1000); ROS_INFO("zuobiao3: %f, %f, %f", x_mm3, y_mm3, z_mm3); geometry_msgs::Pose target_pose4; target_pose4.position.z= 0.109+0.01236-0.0078+0.005; double roll4 = 180*M_PI/180; // 绕 x 轴旋转的弧度 double pitch4 =0*M_PI/180; // 绕 y 轴旋转的弧度 double yaw4 = yaw3; // 绕 z 轴旋转的弧度 float x_mm4 = static_cast<float>(target_pose3.position.x * 1000); float y_mm4 = static_cast<float>(target_pose3.position.y * 1000); float z_mm4 = static_cast<float>(target_pose4.position.z * 1000); ROS_INFO("zuobiao4: %f, %f, %f", x_mm4, y_mm4, z_mm4); // 调用 move_lineb_test 函数并传递坐标 ros::NodeHandle nh; ros::ServiceClient move_lineb_client_ = nh.serviceClient<xarm_msgs::Move>("/xarm/move_lineb"); xarm_msgs::Move move_srv_; if(move_lineb_test(move_srv_, move_lineb_client_, x_mm0, y_mm0, z_mm0, roll0, pitch0, yaw0, x_mm1, y_mm1, z_mm1, roll1, pitch1, yaw1, x_mm2, y_mm2, z_mm2, roll2, pitch2, yaw2, x_mm3, y_mm3, z_mm3, roll3, pitch3, yaw3, x_mm4, y_mm4, z_mm4, roll4, pitch4, yaw4) == 1) return; control_suction_during_move(x_mm4, y_mm4, z_mm4); flag_start++; } else {ros::shutdown(); } } void Cam_RGB_Callback(const sensor_msgs::ImageConstPtr &msg)// 摄像头回调函数 { using namespace cv; image_sub_.shutdown(); // 定义一个cv_bridge指针 cv_bridge::CvImagePtr cv_ptr; try { // 将ROS图像转换为OpenCV图像 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception &e) { ROS_ERROR("cv_bridge exception:%s", e.what()); } // 获取原始图像 Mat imgOriginal = cv_ptr->image; // 定义亮度增强因子 double brightness_scale = 1.8; // 应用亮度增强 Mat brightened; imgOriginal.convertTo(brightened, -1, brightness_scale); // 图像预处理:高斯模糊 Mat blurred; GaussianBlur(brightened, blurred, Size(5, 5), 0); Mat hsv; cvtColor(blurred, hsv, cv::COLOR_BGR2HSV); // 将原始图像转换为HSV图像 // 分离HSV通道 std::vector<Mat> hsv_channels; split(hsv, hsv_channels); // 增强饱和度(S 通道) double saturation_scale = 1.5; // 饱和度增强因子,可以根据实际情况调整 hsv_channels[1].convertTo(hsv_channels[1], -1, saturation_scale); // 合并通道 merge(hsv_channels, hsv); Mat mask_red, mask_green,mask_blue,mask_orange,mask_brown,mask_yellow,mask_purple; // inRange(blurred, cv::Scalar(0, 0, 130), cv::Scalar(255, 108, 226), mask_red); inRange(blurred, cv::Scalar(0, 0, 144), cv::Scalar(252, 111, 203), mask_red); inRange(blurred, cv::Scalar(0, 112, 174), cv::Scalar(159, 139, 237), mask_orange);//BGR inRange(blurred, cv::Scalar(0, 0, 0), cv::Scalar(123, 112, 108), mask_brown); inRange(blurred, cv::Scalar(0, 127, 97), cv::Scalar(157, 255, 136), mask_green); inRange(blurred, cv::Scalar(156, 119, 0), cv::Scalar(218, 146, 102), mask_blue); inRange(blurred, cv::Scalar(0, 142, 152), cv::Scalar(150, 255, 255), mask_yellow); inRange(hsv, cv::Scalar(99, 33, 120), cv::Scalar(134, 146, 181), mask_purple); Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(5, 5));// 形态学操作的内核大小 //dilate(mask_red, mask_red, kernel); //dilate(mask_green, mask_green, kernel); erode(mask_red, mask_red, kernel);//腐蚀 erode(mask_green, mask_green, kernel); erode(mask_blue, mask_blue, kernel); erode(mask_yellow, mask_yellow, kernel); erode(mask_orange, mask_orange, kernel); erode(mask_purple, mask_purple, kernel); erode(mask_brown, mask_brown, kernel); //erode(mask_purple, mask_purple, kernel); dilate(mask_orange, mask_orange, kernel); dilate(mask_brown, mask_brown, kernel); //imshow("green", mask_green);//显示原始图像 //获取储存不同颜色的灰度图 std::vector<std::vector<cv::Point>> contours_red; std::vector<std::vector<cv::Point>> contours_red_output; cv::findContours(mask_red,contours_red, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector<std::vector<cv::Point>> contours_orange; std::vector<std::vector<cv::Point>> contours_orange_output; cv::findContours(mask_orange,contours_orange, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector<std::vector<cv::Point>> contours_brown; std::vector<std::vector<cv::Point>> contours_brown_output; cv::findContours(mask_brown,contours_brown, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector<std::vector<cv::Point>> contours_green; std::vector<std::vector<cv::Point>> contours_green_output; cv::findContours(mask_green,contours_green, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector<std::vector<cv::Point>> contours_blue; std::vector<std::vector<cv::Point>> contours_blue_output; cv::findContours(mask_blue,contours_blue, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector<std::vector<cv::Point>> contours_yellow; std::vector<std::vector<cv::Point>> contours_yellow_output; cv::findContours(mask_yellow,contours_yellow, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector<std::vector<cv::Point>> contours_purple; std::vector<std::vector<cv::Point>> contours_purple_output; cv::findContours(mask_purple,contours_purple, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); //drawContours(blurred, contours, -1, Scalar(0, 255, 0), 3);//轮廓 /********************************红色********************/ for (size_t i = 0; i < contours_red.size(); i++) { std::vector<cv::Point>& contour_red = contours_red[i]; if (contour_red.size()>200) { // 检查轮廓是否为空 contours_red_output.push_back(contours_red[i]); } else { ROS_INFO("No red contours found."); } } /********************************橙色********************/ for (size_t i = 0; i < contours_orange.size(); i++) { std::vector<cv::Point>& contour_orange = contours_orange[i]; if (contour_orange.size()>200) { // 检查轮廓是否为空 contours_orange_output.push_back(contours_orange[i]); } else { ROS_INFO("No orange contours found."); } } /********************************棕色********************/ for (size_t i = 0; i < contours_brown.size(); i++) { std::vector<cv::Point>& contour_brown = contours_brown[i]; if (contour_brown.size()>200) { // 检查轮廓是否为空 contours_brown_output.push_back(contours_brown[i]); } else { ROS_INFO("No brown contours found."); } } /********************************绿色********************/ for (size_t i = 0; i < contours_green.size(); i++) { std::vector<cv::Point>& contour_green = contours_green[i]; if (contour_green.size()>200) { // 检查轮廓是否为空 contours_green_output.push_back(contours_green[i]); } else { ROS_INFO("No green contours found."); } } /********************************蓝色********************/ for (size_t i = 0; i < contours_blue.size(); i++) { std::vector<cv::Point>& contour_blue = contours_blue[i]; if (contour_blue.size()>200) { // 检查轮廓是否为空 contours_blue_output.push_back(contours_blue[i]); } else { ROS_INFO("No blue contours found."); } } /********************************黄色********************/ for (size_t i = 0; i < contours_yellow.size(); i++) { std::vector<cv::Point>& contour_yellow = contours_yellow[i]; if (contour_yellow.size()>200) { // 检查轮廓是否为空 contours_yellow_output.push_back(contours_yellow[i]); } else { ROS_INFO("No yellow contours found."); } } /********************************紫色********************/ // 存储每个轮廓的面积及其索引 std::vector<std::pair<double, size_t>> area_index_pairs; for (size_t i = 0; i < contours_purple.size(); ++i) { double area = cv::contourArea(contours_purple[i]); area_index_pairs.emplace_back(area, i); } // 按面积从大到小排序 std::sort(area_index_pairs.begin(), area_index_pairs.end(), [](const std::pair<double, size_t>& a, const std::pair<double, size_t>& b) { return a.first > b.first; }); // 处理面积最大的5个轮廓 size_t count = std::min<size_t>(5, area_index_pairs.size()); if (count == 0) { ROS_INFO("No contours found."); } for (size_t i = 0; i < count; ++i) { size_t index = area_index_pairs[i].second; contours_purple_output.push_back(contours_purple[index]); } //传递颜色灰度图像 Camera_TO_Robot_Process_YP(contours_purple_output,0,3); Camera_TO_Robot_Process_RO(contours_orange_output,0,3); Camera_TO_Robot_Process_RO(contours_red_output,0,3); Camera_TO_Robot_Process_YP(contours_yellow_output,0,4); Camera_TO_Robot_Process_brown(contours_brown_output,0,2); Camera_TO_Robot_Process_YP(contours_purple_output,3,4); Camera_TO_Robot_Process_GB(contours_blue_output,0,4); Camera_TO_Robot_Process_RO(contours_orange_output,3,4); Camera_TO_Robot_Process_GB(contours_green_output,0,4); Camera_TO_Robot_Process_YP(contours_yellow_output,4,5); Camera_TO_Robot_Process_RO(contours_red_output,3,5); Camera_TO_Robot_Process_brown(contours_brown_output,2,4); Camera_TO_Robot_Process_RO(contours_orange_output,4,5); Camera_TO_Robot_Process_GB(contours_blue_output,4,5); Camera_TO_Robot_Process_YP(contours_purple_output,4,5); Camera_TO_Robot_Process_GB(contours_green_output,4,5); // imshow("imgOriginal", imgOriginal); std::cout << "success!" << std::endl; ros::shutdown(); } //红色和橙色 void Camera_TO_Robot_Process_RO(const std::vector<std::vector<cv::Point>>& contours, int start_number, int end_number) { std::cout << "Red or Orange" << std::endl; cv::Point2f center; for (start_number; start_number < end_number; start_number++) { const std::vector<cv::Point>& contour = contours[start_number]; if (contour.size()>300) // 检查轮廓是否为空 { // 获取最小外接圆 // float radius; // cv::minEnclosingCircle(contours[i], center, radius); // 获取最小外接矩形 cv::RotatedRect minRect = cv::minAreaRect(contours[start_number]); center = minRect.center; if(center.y<60) { center.y=60; } if(center.x<82) { center.x=82; } if(center.y>=60&¢er.x>=82) { cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x, center.y,1); //cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x*1.2-91.0, center.y*1.275-74.5,1); std::cout << pixelPointMat << std::endl; // pixelPointMat = (cv::Mat_<double>(3, 1) << 640, 360, 1); cameraPointMat = objectHeight * K.inv() * pixelPointMat; // std::cout << "相机坐标系下的三维坐标:" << std::endl; // std::cout << cameraPointMat << std::endl; obj_camera_frame1.setX(cameraPointMat.at<double>(0,0)); obj_camera_frame1.setY(cameraPointMat.at<double>(1,0)); obj_camera_frame1.setZ(cameraPointMat.at<double>(2,0)); obj_robot_frame = camera_to_robot_ * obj_camera_frame1; // ros::shutdown(); //std::cout << "11111" << std::endl; /**************** 获取矩形的角度*******************/ double angle = minRect.angle; cv::Size2f size = minRect.size; double width = size.width; double height = size.height; //放平角度 if (width < height) { angle += 90; } // 输出角度 // std::cout << "Contour #" << start_number << " angle: " << angle << std::endl; /**************** 获取矩形的角度*******************/ Grasping(obj_robot_frame.getX(),obj_robot_frame.getY(),obj_robot_frame.getZ(),angle); } } else if(contour.size()>0&&contour.size()<100) { std::cout << "没有红橙像素坐标:" << std::endl; } } } //棕色 void Camera_TO_Robot_Process_brown(const std::vector<std::vector<cv::Point>>& contours, int start_number, int end_number) { std::cout << "Brown start" << std::endl; cv::Point2f center; for (start_number; start_number < end_number; start_number++) { const std::vector<cv::Point>& contour = contours[start_number]; if (contour.size()>300) // 检查轮廓是否为空 { // 获取最小外接圆 float radius; cv::minEnclosingCircle(contours[start_number], center, radius); /**************** 获取矩形的角度*******************/ // 获取最小外接矩形 cv::RotatedRect minRect = cv::minAreaRect(contours[start_number]); double angle2 = minRect.angle; cv::Size2f size = minRect.size; double width = size.width; double height = size.height; /**************** 获取矩形的角度*******************/ double epsilon = 0.1 * cv::arcLength(contours[start_number], true); std::cout << "1111" << std::endl; std::vector<int> lenth(8); std::vector<cv::Point2f> approx; cv::Point2f pt; cv::Point2f pt1; cv::Point2f pt0; cv::approxPolyDP(contours[start_number], approx, epsilon, true); pt0 = approx[0]; if (approx.size() == 3) { for (size_t j = 0; j < approx.size(); j++) { pt= approx[j]; pt1= approx[j+1]; lenth[j]=(pt.x-pt1.x)*(pt.x-pt1.x)+(pt.y-pt1.y)*(pt.y-pt1.y); if(j==2) { lenth[j]=(pt.x-pt0.x)*(pt.x-pt0.x)+(pt.y-pt0.y)*(pt.y-pt0.y); } } if ( lenth[0]>lenth[1]&&lenth[0]>lenth[2]) { if(width > height){ switch (start_number) { case 0: angle2 = angle2; break; case 1: angle2 -= 90; break; case 2: angle2 = angle2; break; case 3: angle2 -= 90; break; } } else if(width < height){ switch (start_number) { case 0: angle2 -= 90; break; case 1: angle2 += 180; break; case 2: angle2 -= 90; break; case 3: angle2 += 180; break; } } } else if ( lenth[2]>lenth[1]&&lenth[2]>lenth[0]) { if(width > height){ switch (start_number) { case 0: angle2 += 180; break; case 1: angle2 += 90; break; case 2: angle2 += 180; break; case 3: angle2 += 90; break; } } else if(width < height){ switch (start_number) { case 0: angle2 += 90; break; case 1: angle2 = angle2; break; case 2: angle2 += 90; break; case 3: angle2 = angle2; break; } } } } // center = minRect.center; if(center.y<60) { center.y=60; } if(center.x<82) { center.x=82; } if(center.y>=60&¢er.x>=82) { //cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x*1.2-91.5, center.y*1.275-74.5,1); //cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x*1.2-92, center.y*1.285-76.5,1); cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x, center.y,1); std::cout << pixelPointMat << std::endl; // pixelPointMat = (cv::Mat_<double>(3, 1) << 640, 360, 1); cameraPointMat = objectHeight * K.inv() * pixelPointMat; // std::cout << "相机坐标系下的三维坐标:" << std::endl; // std::cout << cameraPointMat << std::endl; obj_camera_frame1.setX(cameraPointMat.at<double>(0,0)); obj_camera_frame1.setY(cameraPointMat.at<double>(1,0)); obj_camera_frame1.setZ(cameraPointMat.at<double>(2,0)); obj_robot_frame = camera_to_robot_ * obj_camera_frame1; if(angle2>176) angle2=176; if(angle2<-176) angle2=-176; std::cout << " angle2 :" << angle2 << std::endl; Grasping(obj_robot_frame.getX(),obj_robot_frame.getY(),obj_robot_frame.getZ(),angle2); } } else if(contour.size()>0&&contour.size()<100) { std::cout << "没有棕色像素坐标:" << std::endl; }} } //紫色和黄色 void Camera_TO_Robot_Process_YP(const std::vector<std::vector<cv::Point>>& contours, int start_number, int end_number) { std::cout << "Yellow or Purple start" << std::endl; double angle2=0; for (start_number; start_number < end_number; start_number++) { const std::vector<cv::Point>& contour = contours[start_number]; if (contour.size()>300) // 检查轮廓是否为空 { /**************** 获取矩形的角度*******************/ cv::RotatedRect minRect = cv::minAreaRect(contours[start_number]); angle2 = minRect.angle; cv::Size2f size = minRect.size; double width = size.width; double height = size.height; /**************** 获取矩形的角度*******************/ cv::Point2f center; cv::Point2f center2; double epsilon = 0.04 * cv::arcLength(contours[start_number], true); // 多边形逼近 std::vector<int> lenth(6); std::vector<cv::Point2f> approx; cv::Point2f pt; cv::Point2f pt1; cv::Point2f pt0; cv::approxPolyDP(contours[start_number], approx, epsilon, true); // std::cout <<" lenth[i] "<< approx.size() << std::endl; pt0 = approx[0]; if (approx.size() == 6) { for (size_t i = 0; i < approx.size(); i++) { pt= approx[i]; pt1= approx[i+1]; lenth[i]=(pt.x-pt1.x)*(pt.x-pt1.x)+(pt.y-pt1.y)*(pt.y-pt1.y); if(i==5) lenth[i]=(pt.x-pt0.x)*(pt.x-pt0.x)+(pt.y-pt0.y)*(pt.y-pt0.y); } /********************************purple***************************************/ if ( lenth[5]>lenth[1]&&lenth[5]>lenth[0]&&lenth[5]>lenth[2]&&lenth[5]>lenth[3]&&lenth[5]>lenth[4]) { // center2 = (approx[1]+approx[2])/2; // center = (approx[5]+center2)/2; center = (approx[5]+approx[2])/2; if(width > height){ switch (start_number) { case 0: angle2 -= 90; break; case 1: angle2 += 90; break; case 2: angle2 -= 90; break; case 3: angle2 += 90; break; case 4: angle2 = angle2; break; }} else if(width < height){ switch (start_number) { case 0: angle2 += 180; break; case 1: angle2 = angle2; break; case 2: angle2 += 180; break; case 3: angle2 = angle2; break; case 4: angle2 -= 90; break; }} std::cout << "555555" << std::endl; } /********************************purple***************************************/ else if ( lenth[1]>lenth[2]&&lenth[1]>lenth[0]&&lenth[1]>lenth[5]&&lenth[1]>lenth[3]&&lenth[1]>lenth[4]) { //center2 = (approx[3]+approx[4])/2; //center = (approx[1]+center2)/2; center = (approx[1]+approx[4])/2; if(width > height){ if(approx[1].y<approx[0].y&&approx[1].y<approx[2].y&&approx[1].y<approx[3].y&&approx[1].y<approx[4].y&&approx[1].y<approx[5].y){ switch (start_number) { case 0: angle2 += 90; break; case 1: angle2 -= 90; break; case 2: angle2 += 90; break; case 3: angle2 -= 90; break; case 4: angle2 += 180; break; }} else{ switch (start_number) { case 0: angle2 -= 90; break; case 1: angle2 += 90; break; case 2: angle2 -= 90; break; case 3: angle2 += 90; break; case 4: angle2 = angle2; break; } } } else if(width < height) { switch (start_number) { case 0: angle2 = angle2; break; case 1: angle2 += 180; break; case 2: angle2 = angle2; break; case 3: angle2 += 180; break; case 4: angle2 += 90; break; }} std::cout << "11111111" << std::endl; } /********************************purple***************************************/ /********************************yellow***************************************/ else if ( lenth[0]>lenth[1]&&lenth[0]>lenth[2]&&lenth[0]>lenth[5]&&lenth[0]>lenth[3]&&lenth[0]>lenth[4]) { //center2 = (approx[4]+approx[5])/2; //center = (center2+approx[1])/2; center = (approx[4]+approx[1])/2; if(width < height){ switch (start_number) { case 0: angle2 += 180; break; case 1: angle2 = angle2; break; case 2: angle2 += 180; break; case 3: angle2 = angle2; break; case 4: angle2 += 90; break; } } else{ switch (start_number) { case 0: angle2 -= 90; break; case 1: angle2 += 90; break; case 2: angle2 -= 90; break; case 3: angle2 += 90; break; case 4: angle2 += 180; break; } } std::cout << "000000" << std::endl; } /********************************yellow***************************************/ else if ( lenth[4]>lenth[1]&&lenth[4]>lenth[0]&&lenth[4]>lenth[5]&&lenth[4]>lenth[3]&&lenth[4]>lenth[2])/////yellow { //center2 = (approx[2]+approx[3])/2; // center = (center2+approx[5])/2; center = (approx[2]+approx[5])/2; if(width < height){ if(approx[5].y<approx[0].y&&approx[5].y<approx[1].y&&approx[5].y<approx[2].y&&approx[5].y<approx[3].y&&approx[5].y<approx[4].y){ switch (start_number) { case 0: angle2 = angle2; break; case 1: angle2 += 180; break; case 2: angle2 = angle2; break; case 3: angle2 += 180; break; case 4: angle2 -= 90; break; } } else { switch (start_number) { case 0: angle2 += 180; break; case 1: angle2 = angle2; break; case 2: angle2 += 180; break; case 3: angle2 = angle2; break; case 4: angle2 += 90; break; } } } else if(width > height){ switch (start_number) { case 0: angle2 += 90; break; case 1: angle2 -= 90; break; case 2: angle2 += 90; break; case 3: angle2 -= 90; break; case 4: angle2 = angle2; break; } } std::cout << "444444" << std::endl; } /********************************yellow***************************************/ } if(center.y<60) { center.y=60; } if(center.x<82) { center.x=82; } if(center.y>=60&¢er.x>=82) { //cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x*1.2-92, center.y*1.285-76.5,1); cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x, center.y,1); // std::cout << pixelPointMat << std::endl; // pixelPointMat = (cv::Mat_<double>(3, 1) << 640, 360, 1); cameraPointMat = objectHeight * K.inv() * pixelPointMat; // std::cout << "相机坐标系下的三维坐标:" << std::endl; // std::cout << cameraPointMat << std::endl; obj_camera_frame1.setX(cameraPointMat.at<double>(0,0)); obj_camera_frame1.setY(cameraPointMat.at<double>(1,0)); obj_camera_frame1.setZ(cameraPointMat.at<double>(2,0)); obj_robot_frame = camera_to_robot_ * obj_camera_frame1; std::cout<< " X :" << obj_robot_frame.getX() << std::endl; std::cout<< " Y :" << obj_robot_frame.getY() << std::endl; std::cout<< " Z :" << obj_robot_frame.getZ() << std::endl; if(angle2>176) angle2=176; if(angle2<-176) angle2=-176; std::cout << " angle2 :" << angle2 << std::endl; Grasping(obj_robot_frame.getX(),obj_robot_frame.getY(),obj_robot_frame.getZ(),angle2); } } else if(contour.size()>0&&contour.size()<280) { std::cout << "没有黄紫像素坐标:" << std::endl; }} } //绿色和蓝色 void Camera_TO_Robot_Process_GB(const std::vector<std::vector<cv::Point>>& contours, int start_number, int end_number) { std::cout << "green or blue" << std::endl; // 输出提示信息 cv::Point2f center; for(start_number; start_number < end_number; start_number++) { const std::vector<cv::Point>& contour = contours[start_number]; if (contour.size()>300) { // 检查轮廓是否为空 // 获取最小外接矩形 cv::RotatedRect minRect = cv::minAreaRect(contours[start_number]); center = minRect.center; double angleInRadians = minRect.angle * M_PI / 180.0; if(center.y<60) { center.y=60; } if(center.x<82) { center.x=82; } if(center.y>=60&¢er.x>=82) { cv::Mat pixelPointMat = (cv::Mat_<double>(3, 1) << center.x, center.y,1); std::cout << pixelPointMat << std::endl; cameraPointMat = objectHeight * K.inv() * pixelPointMat; //平面下的坐标转为相机三维坐标 //相机坐标转为基座标 obj_camera_frame1.setX(cameraPointMat.at<double>(0, 0)); obj_camera_frame1.setY(cameraPointMat.at<double>(1, 0)); obj_camera_frame1.setZ(cameraPointMat.at<double>(2, 0)); obj_robot_frame= camera_to_robot_ * obj_camera_frame1; std::cout << "坐标为: " << obj_robot_frame << std::endl; /**************** 获取矩形的角度*******************/ double angle = minRect.angle; cv::Size2f size = minRect.size; double width = size.width; double height = size.height; //放平角度 if (width > height) { angle += 90; } Grasping(obj_robot_frame.getX(),obj_robot_frame.getY(),obj_robot_frame.getZ(),angle); } } else if(contour.size()>0&&contour.size()<100) { std::cout << "没有蓝绿像素坐标:" << std::endl; } } } }; /**********调用服务运行机械臂*********************/ int move_lineb_test(xarm_msgs::Move srv, ros::ServiceClient client, float x_mm0, float y_mm0, float z_mm0, double roll0, double pitch0, double yaw0, float x_mm1, float y_mm1, float z_mm1, double roll1, double pitch1, double yaw1, float x_mm2, float y_mm2, float z_mm2, double roll2, double pitch2, double yaw2, float x_mm3, float y_mm3, float z_mm3, double roll3, double pitch3, double yaw3, float x_mm4, float y_mm4, float z_mm4, double roll4, double pitch4, double yaw4) { // 设置机械臂的运动速度 srv.request.mvvelo = 160; // 设置机械臂的运动加速度 srv.request.mvacc = 1000; // 设置机械臂的运动时间 srv.request.mvtime = 0; // 设置机械臂运动路径的圆角半径 srv.request.mvradii = 20; ROS_INFO("ZUOBIAOR: %f, %f,%f", x_mm1, y_mm1, z_mm1); std::vector<float> pose[5] = { {x_mm0, y_mm0, z_mm0, static_cast<float>(roll0), static_cast<float>(pitch0), static_cast<float>(yaw0)}, {x_mm1, y_mm1, z_mm1, static_cast<float>(roll1), static_cast<float>(pitch1), static_cast<float>(yaw1)}, {x_mm2, y_mm2, z_mm2, static_cast<float>(roll2), static_cast<float>(pitch2), static_cast<float>(yaw2)}, {x_mm3, y_mm3, z_mm3, static_cast<float>(roll3), static_cast<float>(pitch3), static_cast<float>(yaw3)}, {x_mm4, y_mm4, z_mm4, static_cast<float>(roll4), static_cast<float>(pitch4), static_cast<float>(yaw4)} }; for(int i = 0; i < 5; i++) { srv.request.pose = pose[i]; if(client.call(srv)) { ROS_INFO("%s\n", srv.response.message.c_str()); std::cout << "success111" << std::endl; } else { ROS_ERROR("Failed to call service move_lineb"); } } return 0; } bool is_at_pose(const geometry_msgs::Pose& current_pose, const geometry_msgs::Pose& target_pose, double tolerance = 0.005) { return std::abs(current_pose.position.x - target_pose.position.x) < tolerance && std::abs(current_pose.position.y - target_pose.position.y) < tolerance && std::abs(current_pose.position.z - target_pose.position.z) < tolerance; } //阻塞服务,检测是否到达目标位置 void control_suction_during_move(float x_mm4, float y_mm4, float z_mm4) { // 定义目标位 moveit::planning_interface::MoveGroupInterface arm("xarm7"); ros::AsyncSpinner spinner(1); spinner.start(); geometry_msgs::Pose target_pose4; target_pose4.position.x = x_mm4 / 1000.0; target_pose4.position.y = y_mm4 / 1000.0; target_pose4.position.z = (z_mm4-76.7398-13.9+8.5) / 1000.0; bool suction_off_triggered = false; // 循环检查当前位姿 ros::Rate rate(10); // 10 Hz 检查频率 while (ros::ok()) { geometry_msgs::Pose current_pose = arm.getCurrentPose().pose; // ROS_INFO("ZUOBIAO_SHISHI: %f, %f, %f", current_pose.position.x, current_pose.position.y, current_pose.position.z); // 检查是否到达关闭吸盘的位置 if (!suction_off_triggered && is_at_pose(current_pose, target_pose4)) { ROS_INFO("Reached suction off position, trying to turn off suction."); sleep(0.8); writeSpeed(0); writeSpeed(0); writeSpeed(0); sleep(0.5); ROS_INFO("Suction off commands sent."); suction_off_triggered = true; } // 如果两个条件都满足,退出循环 if (suction_off_triggered) { break; } rate.sleep(); } } int main(int argc, char** argv) { // 初始化ROS节点 ros::init(argc, argv, "xarm_api"); ros::NodeHandle nh; XArmAPItest ic; nh.setParam("/xarm/wait_for_finish", true); ros::Publisher sleep_pub_ = nh.advertise<std_msgs::Float32>("/xarm/sleep_sec", 1); ros::ServiceClient motion_ctrl_client_ = nh.serviceClient<xarm_msgs::SetAxis>("/xarm/motion_ctrl"); ros::ServiceClient set_mode_client_ = nh.serviceClient<xarm_msgs::SetInt16>("/xarm/set_mode"); ros::ServiceClient set_state_client_ = nh.serviceClient<xarm_msgs::SetInt16>("/xarm/set_state"); ros::ServiceClient move_lineb_client_ = nh.serviceClient<xarm_msgs::Move>("/xarm/move_lineb"); xarm_msgs::SetAxis set_axis_srv_; xarm_msgs::SetInt16 set_int16_srv_; xarm_msgs::Move move_srv_; float x_mm0, y_mm0, z_mm0; double roll0, pitch0, yaw0; float x_mm1, y_mm1, z_mm1; double roll1, pitch1, yaw1; float x_mm2, y_mm2, z_mm2; double roll2, pitch2, yaw2; float x_mm3, y_mm3, z_mm3; double roll3, pitch3, yaw3; float x_mm4, y_mm4, z_mm4; double roll4, pitch4, yaw4; set_axis_srv_.request.id = 8; set_axis_srv_.request.data = 1; if(motion_ctrl_client_.call(set_axis_srv_)) { ROS_INFO("%s\n", set_axis_srv_.response.message.c_str()); } else { ROS_ERROR("Failed to call service motion_ctrl"); return 1; } set_int16_srv_.request.data = 0; if(set_mode_client_.call(set_int16_srv_)) { ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); } else { ROS_ERROR("Failed to call service set_mode"); return 1; } set_int16_srv_.request.data = 0; if(set_state_client_.call(set_int16_srv_)) { ROS_INFO("%s\n", set_int16_srv_.response.message.c_str()); } else { ROS_ERROR("Failed to call service set_state"); return 1; } nh.setParam("/xarm/wait_for_finish", false); std_msgs::Float32 sleep_msg; sleep_msg.data = 1.0; sleep_pub_.publish(sleep_msg); if(move_lineb_test(move_srv_, move_lineb_client_, x_mm0, y_mm0, z_mm0, roll0, pitch0, yaw0, x_mm1, y_mm1, z_mm1, roll1, pitch1, yaw1, x_mm2, y_mm2, z_mm2, roll2, pitch2, yaw2, x_mm3, y_mm3, z_mm3, roll3, pitch3, yaw3, x_mm4, y_mm4, z_mm4, roll4, pitch4, yaw4) == 1) return 1; // 调用 control_suction_during_move 函数 control_suction_during_move(x_mm4, y_mm4, z_mm4); sleep(0.8); nh.setParam("/xarm/wait_for_finish", true); while(ros::ok()) { ros::spinOnce(); } return 0; }解释一下

#include <chrono> #include <functional> #include <memory> #include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/trigger.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" using namespace std::chrono_literals; class ShelfDetector : public rclcpp::Node { public: ShelfDetector() : Node("shelf_detector") { // 创建服务响应上位机请求 service_ = create_service<std_srvs::srv::Trigger>( "detect_shelf", std::bind(&ShelfDetector::handle_detect_request, this, std::placeholders::_1, std::placeholders::_2)); // 创建货架位姿发布者 pose_publisher_ = create_publisher<geometry_msgs::msg::PoseStamped>("shelf_pose", 10); RCLCPP_INFO(get_logger(), "Shelf Detector ready. Waiting for detection request..."); } private: void handle_detect_request( const std::shared_ptr<std_srvs::srv::Trigger::Request> request, std::shared_ptr<std_srvs::srv::Trigger::Response> response) { (void)request; // 避免未使用参数警告 RCLCPP_INFO(get_logger(), "Received detection request. Starting shelf detection..."); // 模拟检测过程(实际应替换为真实检测逻辑) bool detection_success = false; auto start_time = now(); rclcpp::Rate loop_rate(10); // 10Hz检测频率 // 循环等待检测结果(最多5秒) while (rclcpp::ok() && (now() - start_time) < 5s) { // 模拟检测逻辑(真实场景使用传感器数据) if ((now() - start_time) > 2s) { // 2秒后"检测到"货架 detection_success = true; break; } RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "Detecting shelf..."); loop_rate.sleep(); } if (detection_success) { // 发布检测到的货架位姿 auto pose = geometry_msgs::msg::PoseStamped(); pose.header.stamp = now(); pose.header.frame_id = "map"; pose.pose.position.x = 1.5; pose.pose.position.y = 3.2; pose.pose.orientation.w = 1.0; pose_publisher_->publish(pose); response->success = true; response->message = "Shelf detected at x:1.5, y:3.2"; RCLCPP_INFO(get_logger(), "Detection succeeded. Pose published."); } else { response->success = false; response->message = "Detection timeout"; RCLCPP_ERROR(get_logger(), "Detection failed!"); } } rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_; rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_; }; int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared<ShelfDetector>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }基于该段代码,扩充将laserscan转换为pointcloud后重新发布,并通过订阅转换后的点云在rviz中查看,增加实时位姿/fusion_pose的订阅,并将最近位姿存储即latest_pose = *msg,增加收到客户端调用时先回复收到请求,然后performShelfDetection计算货架中心位姿并发布,订阅后在rviz中可视化

. ├── cliff_distance_measurement │ ├── CMakeLists.txt │ ├── include │ │ └── cliff_distance_measurement │ ├── package.xml │ └── src │ ├── core │ ├── ir_ranging.cpp │ └── platform ├── robot_cartographer │ ├── config │ │ └── fishbot_2d.lua │ ├── map │ │ ├── fishbot_map.pgm │ │ └── fishbot_map.yaml │ ├── package.xml │ ├── readme.md │ ├── resource │ │ └── robot_cartographer │ ├── robot_cartographer │ │ ├── __init__.py │ │ └── robot_cartographer.py │ ├── rviz │ ├── setup.cfg │ └── setup.py ├── robot_control_service │ ├── bash │ │ └── pwm_control_setup.sh │ ├── CMakeLists.txt │ ├── config │ │ └── control_params.yaml │ ├── include │ │ └── robot_control_service │ ├── package.xml │ ├── readme.md │ └── src │ ├── control_client_camera.cpp │ ├── control_client_cliff.cpp │ ├── control_client_ir.cpp │ ├── control_client_ir_four.cpp │ ├── control_client_master.cpp │ ├── control_client_ros.cpp │ ├── control_client_ultrasonic.cpp │ ├── control_service.cpp │ ├── DirectMotorControl.cpp │ ├── PIDControl.cpp │ ├── publisher_control_view.cpp │ └── publisher_human_realized.cpp ├── robot_control_view │ ├── config │ │ └── icare_robot.rviz │ ├── __init__.py │ ├── launch │ │ └── start_init_view.launch.py │ ├── package.xml │ ├── resource │ │ └── robot_control_view │ ├── robot_control_view │ │ ├── app │ │ ├── blood_oxygen_pulse │ │ ├── __init__.py │ │ ├── __pycache__ │ │ ├── robot_automatic_cruise_server.py │ │ ├── robot_automatic_recharge_server.py │ │ ├── robot_automatic_slam_server.py │ │ ├── robot_blood_oxygen_pulse.py │ │ ├── robot_city_locator_node.py │ │ ├── robot_control_policy_server.py │ │ ├── robot_local_websocket.py │ │ ├── robot_log_clear_node.py │ │ ├── robot_main_back_server.py │ │ ├── robot_network_publisher.py │ │ ├── robot_network_server.py │ │ ├── robot_odom_publisher.py │ │ ├── robot_speech_server.py │ │ ├── robot_system_info_node.py │ │ ├── robot_ultrasonic_policy_node.py │ │ ├── robot_view_manager_node.py │ │ ├── robot_websockets_client.py │ │ ├── robot_websockets_server.py │ │ ├── robot_wifi_server_node.py │ │ ├── start_account_view.py │ │ ├── start_bluetooth_view.py │ │ ├── start_chat_view.py │ │ ├── start_clock_view.py │ │ ├── start_feedback_view.py │ │ ├── start_health_view.py │ │ ├── start_init_view.py │ │ ├── start_lifecycle_view.py │ │ ├── start_main_view.py │ │ ├── start_member_view.py │ │ ├── start_movie_view.py │ │ ├── start_music_view.py │ │ ├── start_radio_view.py │ │ ├── start_schedule_view.py │ │ ├── start_setting_view.py │ │ ├── start_test_view.py │ │ ├── start_view_manager.py │ │ ├── start_weather_view.py │ │ └── start_wifi_view.py │ ├── setup.cfg │ ├── setup.py │ ├── test │ │ ├── my_test.py │ │ ├── test_copyright.py │ │ ├── test_flake8.py │ │ └── test_pep257.py │ └── urdf │ ├── first_robot.urdf.xacro │ ├── fishbot.urdf │ ├── fishbot.urdf.xacro │ ├── fist_robot.urdf │ ├── icare_robot.urdf │ ├── icare_robot.urdf.xacro │ ├── ramand.md │ └── xacro_template.xacro ├── robot_costmap_filters │ ├── CMakeLists.txt │ ├── include │ │ └── robot_costmap_filters │ ├── launch │ │ ├── start_costmap_filter_info_keepout.launch.py │ │ ├── start_costmap_filter_info.launch.py │ │ └── start_costmap_filter_info_speedlimit.launch.py │ ├── package.xml │ ├── params │ │ ├── filter_info.yaml │ │ ├── filter_masks.yaml │ │ ├── keepout_mask.pgm │ │ ├── keepout_mask.yaml │ │ ├── keepout_params.yaml │ │ ├── speedlimit_params.yaml │ │ ├── speed_mask.pgm │ │ └── speed_mask.yaml │ ├── readme.md │ └── src ├── robot_description │ ├── launch │ │ └── gazebo.launch.py │ ├── package.xml │ ├── readme.md │ ├── resource │ │ └── robot_description │ ├── robot_description │ │ └── __init__.py │ ├── rviz │ │ └── urdf_config.rviz │ ├── setup.cfg │ ├── setup.py │ ├── urdf │ │ ├── fishbot_gazebo.urdf │ │ ├── fishbot_v0.0.urdf │ │ ├── fishbot_v1.0.0.urdf │ │ ├── test.urdf │ │ └── three_wheeled_car_model.urdf │ └── worlds │ └── empty_world.world ├── robot_interfaces │ ├── CMakeLists.txt │ ├── include │ │ └── robot_interfaces │ ├── msg │ │ ├── AlarmClockMsg.msg │ │ ├── CameraMark.msg │ │ ├── DualRange.msg │ │ ├── HuoerSpeed.msg │ │ ├── IrSensorArray.msg │ │ ├── IrSignal.msg │ │ ├── NavigatorResult.msg │ │ ├── NavigatorStatus.msg │ │ ├── NetworkDataMsg.msg │ │ ├── PoseData.msg │ │ ├── RobotSpeed.msg │ │ ├── SensorStatus.msg │ │ ├── TodayWeather.msg │ │ └── WifiDataMsg.msg │ ├── package.xml │ ├── readme.md │ ├── src │ └── srv │ ├── LightingControl.srv │ ├── MotorControl.srv │ ├── NewMotorControl.srv │ ├── SetGoal.srv │ ├── StringPair.srv │ ├── String.srv │ └── VoicePlayer.srv ├── robot_launch │ ├── config │ │ └── odom_imu_ekf.yaml │ ├── launch │ │ ├── start_all_base_sensor.launch.py │ │ ├── start_cartographer.launch.py │ │ ├── start_control_service.launch.py │ │ ├── start_navigation.launch.py │ │ ├── start_navigation_service.launch.py │ │ ├── start_navigation_speed_mask.launch.py │ │ ├── start_navigation_with_speed_and_keepout.launch.py │ │ ├── start_ros2.launch.py │ │ ├── test_camera_2.launch.py │ │ ├── test_camera.launch.py │ │ ├── test_car_model.launch.py │ │ ├── test_cliff.launch.py │ │ ├── test_ir.launch.py │ │ ├── test_self_checking.launch.py │ │ ├── test_video_multiplesing.launch.py │ │ └── test_visualization.launch.py │ ├── package.xml │ ├── readme.md │ ├── resource │ │ └── robot_launch │ ├── robot_launch │ │ └── __init__.py │ ├── setup.cfg │ └── setup.py ├── robot_navigation │ ├── config │ │ ├── nav2_filter.yaml │ │ ├── nav2_params.yaml │ │ └── nav2_speed_filter.yaml │ ├── maps │ │ ├── fishbot_map.pgm │ │ └── fishbot_map.yaml │ ├── package.xml │ ├── readme.md │ ├── resource │ │ └── robot_navigation │ ├── robot_navigation │ │ ├── __init__.py │ │ └── robot_navigation.py │ ├── setup.cfg │ └── setup.py ├── robot_navigation2_service │ ├── package.xml │ ├── readme.md │ ├── resource │ │ └── robot_navigation2_service │ ├── robot_navigation2_service │ │ ├── camera_follower_client.py │ │ ├── go_to_pose_service.py │ │ ├── __init__.py │ │ ├── leave_no_parking_zone_client_test_2.py │ │ ├── pose_init.py │ │ ├── real_time_point_client.py │ │ ├── recharge_point_client.py │ │ ├── repub_speed_filter_mask.py │ │ └── save_pose.py │ ├── setup.cfg │ └── setup.py ├── robot_sensor │ ├── bash │ │ └── isr_brushless.sh │ ├── CMakeLists.txt │ ├── config │ │ └── sensor_params.yaml │ ├── include │ │ └── robot_sensor │ ├── package.xml │ ├── readme.md │ └── src │ ├── robot_battery_state_publisher.cpp │ ├── robot_battery_voltage_publisher.cpp │ ├── robot_charging_status_publisher.cpp │ ├── robot_cliff_distance_publisher.cpp │ ├── robot_encode_speed_publisher.cpp │ ├── robot_imu_publisher.cpp │ ├── robot_ir_four_signal_publisher.cpp │ ├── robot_ir_signal_publisher.cpp │ ├── robot_keyboard_control_publisher.cpp │ ├── robot_lighting_control_server.cpp │ ├── robot_map_publisher.cpp │ ├── robot_odom_publisher.cpp │ ├── robot_smoke_alarm_publisher.cpp │ ├── robot_ultrasonic_publisher.cpp │ └── robot_wireless_alarm_publisher.cpp ├── robot_sensor_self_check │ ├── check_report │ │ ├── sensor_diagnostic_report_20250226_144435.json │ │ ├── sensor_diagnostic_report_20250226_144435.txt │ │ ├── sensor_diagnostic_report_20250226_144850.json │ │ ├── sensor_diagnostic_report_20250226_144850.txt │ │ ├── sensor_diagnostic_report_20250226_144927.json │ │ ├── sensor_diagnostic_report_20250226_144927.txt │ │ ├── sensor_diagnostic_report_20250226_144958.json │ │ └── sensor_diagnostic_report_20250226_144958.txt │ ├── config │ │ └── sensors_config.yaml │ ├── package.xml │ ├── resource │ │ └── robot_sensor_self_check │ ├── robot_sensor_self_check │ │ ├── __init__.py │ │ ├── robot_sensor_self_check.py │ │ └── test_topic.py │ ├── setup.cfg │ ├── setup.py │ └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── robot_visual_identity │ ├── cfg │ │ ├── nanotrack.yaml │ │ ├── rknnconfig.yaml │ │ └── stgcnpose.yaml │ ├── face_feature │ │ ├── mss_face_encoding.npy │ │ ├── wd_face_encoding.npy │ │ └── yls_face_encoding.npy │ ├── package.xml │ ├── resource │ │ ├── robot_visual_identity │ │ └── ros_rknn_infer │ ├── rknn_model │ │ ├── blood_detect.rknn │ │ ├── blood-seg-last-cbam.rknn │ │ ├── face_detect.rknn │ │ ├── face_emotion.rknn │ │ ├── face_keypoint.rknn │ │ ├── face_verify.rknn │ │ ├── head_detect.rknn │ │ ├── nanotrack_backbone127.rknn │ │ ├── nanotrack_backbone255.rknn │ │ ├── nanotrack_head.rknn │ │ ├── people_detect.rknn │ │ ├── stgcn_pose.rknn │ │ ├── yolo_kpt.rknn │ │ └── yolov8s-pose.rknn │ ├── robot_visual_identity │ │ ├── 人体跟随与避障控制系统文档.md │ │ ├── __init__.py │ │ ├── rknn_infer │ │ ├── robot_behavior_recognition.py │ │ ├── robot_emotion_recognition.py │ │ ├── robot_people_rgb_follow.py │ │ ├── robot_people_scan_follow.py │ │ └── robot_people_track.py │ ├── setup.cfg │ ├── setup.py │ └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── video_multiplexing │ ├── bash │ │ ├── test_config.linphonerc │ │ ├── test_video_stream.sh │ │ └── video_stream.pcap │ ├── COLCON_IGNORE │ ├── package.xml │ ├── resource │ │ └── video_multiplexing │ ├── setup.cfg │ ├── setup.py │ ├── test │ │ ├── test_copyright.py │ │ ├── test_flake8.py │ │ └── test_pep257.py │ └── video_multiplexing │ ├── __init__.py │ ├── __pycache__ │ ├── rtp_utils.py │ ├── video_freeswitch.py │ ├── video_linphone_bridge.py │ ├── video_publisher.py │ └── video_test_freeswitch.py └── ydlidar_ros2_driver-humble ├── CMakeLists.txt ├── config │ └── ydlidar.rviz ├── details.md ├── images │ ├── cmake_error.png │ ├── EAI.png │ ├── finished.png │ ├── rviz.png │ ├── view.png │ └── YDLidar.jpg ├── launch │ ├── ydlidar_launch.py │ ├── ydlidar_launch_view.py │ └── ydlidar.py ├── LICENSE.txt ├── package.xml ├── params │ └── TminiPro.yaml ├── README.md ├── src │ ├── ydlidar_ros2_driver_client.cpp │ └── ydlidar_ros2_driver_node.cpp └── startup └── initenv.sh 93 directories, 299 files 我的机器人ros2系统是有显示和主控页面的居家服务型移动机器人,用户点击下载更新就开始执行更新流程,整个系统更新功能应该怎么设计,在开发者应该编写哪些代码和做哪些准备,如何设计流程

最新推荐

recommend-type

公路工程机械设计制造与自动化发展方向.docx

公路工程机械设计制造与自动化发展方向.docx
recommend-type

电子商务实训心得体会范文.docx

电子商务实训心得体会范文.docx
recommend-type

Excel表格通用模板:成品管理表格.xls

Excel表格通用模板:成品管理表格.xls
recommend-type

pygtk2-codegen-2.24.0-24.module_el8.0.0+36+bb6a76a2.tar.gz

# 适用操作系统:Centos8 #Step1、解压 tar -zxvf xxx.el8.tar.gz #Step2、进入解压后的目录,执行安装 sudo rpm -ivh *.rpm
recommend-type

podman-compose-1.0.6-1.el8.tar.gz

# 适用操作系统:Centos8 #Step1、解压 tar -zxvf xxx.el8.tar.gz #Step2、进入解压后的目录,执行安装 sudo rpm -ivh *.rpm
recommend-type

SSRSSubscriptionManager工具:简化SSRS订阅的XML文件导入

### 知识点概述 #### 标题知识点 1. **SSRSSubscriptionManager**: 这是一个专门用于管理SQL Server Reporting Services (SSRS) 订阅的工具或脚本。它允许用户从一个集中的位置管理SSRS订阅。 2. **从XML文件导入SSRS订阅**: 描述了一个通过读取XML文件来配置SSRS订阅的过程。这可能是为了减少重复的手动设置和避免错误,提高管理效率。 #### 描述知识点 3. **快速部署多个SSRS订阅**: 该工具或脚本的一个主要功能是能够快速设置多个订阅,这比传统的SSRS在线向导更为高效。 4. **标准SSRS在线向导的局限性**: 描述了标准SSRS向导的不足之处,例如操作缓慢、单次只能设置一个订阅,以及易于出现人为错误。 5. **SSRS订阅管理器的优势**: 解释了为什么使用SSRS订阅管理器比标准向导更可靠。它允许使用预定义的XML文档进行设置,这些文档可以经过测试和验证以减少错误。 6. **受控文档**: 强调了使用SSRS订阅管理器的一个好处是能够控制订阅设置,使其更为可靠且易于管理。 7. **版本控制和订阅设置**: 讨论了SSRS报告可以进行版本控制,但是传统的订阅设置通常不包含在版本控制中,而SSRS订阅管理器提供了一种方式,可以对这些设置进行记录和控制。 #### 标签知识点 8. **C#**: 指示了实现SSRSSubscriptionManager可能使用的技术,C# 是一种面向对象的编程语言,通常用于开发.NET应用程序,包括SSRS订阅管理器。 #### 压缩包子文件名列表 9. **SSRSSubscriptionManager-master**: 表示这是一个开源项目或组件的主干文件夹。名称表明这是一个版本控制仓库中的主分支,可能包含了源代码、项目文件和其他资源文件。 ### 详细知识点 #### 关于SSRS - SQL Server Reporting Services (SSRS) 是一个服务器基础的报告平台,它能够通过Web界面、文件共享和电子邮件来交付报表内容。SSRS用户可以根据数据源生成数据驱动的报表,并设置订阅以便自动分发这些报表。 - SSRS订阅是一个功能,允许用户根据设定的计划或用户触发条件自动获取报表。订阅可以是快照订阅、数据驱动订阅或基于事件的订阅。 #### 关于SSRSSubscriptionManager - SSRSSubscriptionManager是一个工具,其设计意图是简化SSRS订阅的管理过程。它允许管理员在单个操作中部署大量订阅,相比于传统方法,它极大地节省了时间。 - 通过使用XML文件来定义订阅的设置,该工具提供了更高的准确性和一致性,因为XML文件可以被严格地测试和审核。 - 自动化和批量操作可以减少因手动设置造成的错误,并且提高了操作效率。这对于有大量报表和订阅需求的企业来说尤为重要。 - SSRSSubscriptionManager的出现也表明了开发人员对IT自动化、脚本化操作和管理工具的需求,这可以视为一种持续的向DevOps文化和实践的推进。 #### 关于C# - C# 是一种由微软开发的通用编程语言,它被广泛应用于开发Windows应用程序、服务器端Web应用程序以及移动和游戏开发。 - 在开发SSRSSubscriptionManager时,C# 语言的利用可能涉及到多种.NET框架中的类库,例如System.Xml用于解析和操作XML文件,System.Data用于数据库操作等。 - 使用C# 实现SSRS订阅管理器可以享受到.NET平台的诸多优势,比如类型安全、内存管理和跨平台兼容性。 #### 关于版本控制 - 版本控制是一种记录源代码文件更改历史的方法,它允许开发团队追踪和管理代码随时间的变化。常见的版本控制系统包括Git、Subversion等。 - 在SSRS订阅的上下文中,版本控制意味着可以追踪每个订阅设置的变更,从而保证订阅设置的一致性和可追溯性。 - SSRSSubscriptionManager通过使用XML文件,可以使得版本控制变得更加容易,因为XML文件可以被版本控制系统跟踪。 - 这种做法还确保了订阅设置文件的历史版本可以被审计,对企业的合规性和管理都有积极影响。 ### 结论 SSRSSubscriptionManager通过集成自动化、XML文件和版本控制,为SSRS订阅管理提供了更高效、可信赖和可管理的解决方案。使用C# 实现的这一工具能够极大提高IT专业人员在创建和维护SSRS订阅时的工作效率,并减少可能由手工操作引入的错误。通过强调自动化和可控制的文档处理,它也反映了IT行业的趋势,即追求效率、可靠性和版本管理。
recommend-type

图形缩放与平移实现全攻略:Delphi视图变换核心技术详解

# 摘要 本文系统探讨了图形缩放与平移技术的基本原理及其在实际开发中的应用,涵盖从数学基础到编程实现的全过程。文章首先介绍了图形变换的数学模型,包括坐标系统、矩
recommend-type

Unknown custom element: <CustomForm> - did you register the component correctly? For recursive components, make sure to provide the "name" option.

在使用 Vue.js 时,如果遇到未知自定义组件 `<CustomForm>` 的错误提示,通常是由于组件注册过程中存在某些疏漏或错误。以下是常见的原因及对应的解决方案: ### 1. 组件未正确注册 确保 `<CustomForm>` 组件已经在使用它的父组件或全局中进行了注册。如果未注册,Vue 会提示该组件是未知的。 正确的注册方式如下: - **全局注册**(适用于所有组件都能访问的场景): ```javascript import CustomForm from '@/components/CustomForm.vue' Vue.component('CustomForm',
recommend-type

使用KnockoutJS开发的黑客新闻阅读器 hn-ko

在给定的文件信息中,我们可以提炼出以下IT相关知识点: ### 标题知识点 #### KnockoutJS - **KnockoutJS定义**:Knockout是一个轻量级的JavaScript库,它允许开发者利用声明式绑定方式创建富交互的Web应用程序。它特别擅长于实现UI的自动更新,当模型的数据发生变化时,视图会自动响应这些变化而更新,无需手动操作DOM。 - **KnockoutJS核心特性**: - **依赖项跟踪**:Knockout能够跟踪数据模型中的变化,当数据更新时自动更新相关联的UI元素。 - **声明式绑定**:开发者可以使用简单的数据绑定语法在HTML标记中直接指定数据与DOM元素之间的关系,这样可以使代码更加清晰和易于维护。 - **模板和自定义绑定**:Knockout提供了灵活的模板系统,可以创建可复用的UI组件,并通过自定义绑定来扩展其核心功能,以满足特定需求。 - **组件化**:Knockout支持创建独立的、可复用的视图模型组件,以构建复杂的用户界面。 ### 描述知识点 #### 入门和运行应用 - **Git克隆**:通过`git clone`命令可以从远程仓库克隆代码到本地环境,这是版本控制中常见的操作,有助于团队协作和代码共享。`https://github.com/crissdev/hn-ko.git`指向一个特定的GitHub仓库,其中包含着使用KnockoutJS编写的黑客新闻应用代码。 - **NPM(Node Package Manager)**:NPM是随Node.js一起安装的一个包管理工具,它用于安装和管理JavaScript项目依赖。`npm install`命令用于安装项目中的所有依赖项,这可能包括KnockoutJS库以及其他可能用到的库或框架。 - **启动应用**:`npm start`是启动脚本的命令,它通常在`package.json`文件的scripts部分定义,用以启动开发服务器或运行应用。 #### 麻省理工学院许可证 - **MIT许可证**:这是一种常见的开源许可证,允许用户在任何类型的项目中免费使用软件,无论是个人的还是商业的。在保留原作者版权声明的同时,用户可以根据自己的需要修改和分发代码。这是很多开源项目选择的许可证。 ### 标签知识点 #### JavaScript - **JavaScript作用**:JavaScript是一种高级的、解释执行的编程语言,它通常是运行在浏览器中的脚本语言,用于实现网页的动态效果和用户交互。JavaScript作为全栈开发的关键技术之一,也被广泛用于服务器端开发(Node.js)。 - **JavaScript特点**: - **事件驱动**:JavaScript可以响应用户的点击、输入等事件,并据此进行操作。 - **对象导向**:JavaScript支持面向对象编程,可以通过创建对象、继承、多态等特性来组织代码。 - **异步编程**:JavaScript支持异步编程模型,利用回调函数、Promises、async/await等技术,可以有效处理网络请求、用户输入等异步操作。 ### 压缩包子文件的文件名称列表知识点 - **hn-ko-master**:这表明压缩包中的文件是从名为`hn-ko`的GitHub仓库的`master`分支获取的。文件列表中的这个名称可以帮助开发者快速识别包含KnockoutJS项目的代码仓库版本。 ### 总结 以上知识点总结了文件信息中提及的关于KnockoutJS、Git、NPM、MIT许可证和JavaScript的核心概念和应用实践。KnockoutJS作为一个功能强大的前端库,特别适用于复杂用户界面的数据绑定和动态更新。而通过Git的使用可以方便地管理项目的版本,并与其他开发者协作。NPM则使得项目的依赖管理和模块化开发变得更加简单高效。MIT许可证为项目的使用者提供了法律上的许可,确保了软件使用的自由度。JavaScript作为一种多用途的编程语言,在前端开发中扮演了不可替代的角色。理解并运用这些知识点,将有助于进行现代Web应用的开发工作。
recommend-type

Delphi图层管理机制设计:打造高效绘图控件的架构之道

# 摘要 本文系统研究了Delphi图层管理机制的核心概念、理论基础与实现细节,重点分析了图层的数据模型、渲染流程及其交互机制。通过对图层容器设计、绘制性能优化与事件分发模型的深入探讨,提出了一个高效、可扩展的图层管理架构,并结合实际绘图控件开发,验证了该机制