Eigen::Matrix3d类型的变量R为什么没有成员函数translation

时间: 2024-09-06 22:02:05 浏览: 89
Eigen库是一个强大的线性代数库,特别适用于处理数值计算和矩阵运算,包括三维空间中的旋转(rotation)。`Eigen::Matrix3d`是一个3x3的双精度浮点型矩阵,通常用于表示3D变换,比如旋转、缩放和位移(translation)等。 然而,`Eigen::Matrix3d`本身并没有提供一个名为`translation`的成员函数,因为它主要用于表示旋转,而不是直接存储位移信息。如果你想要在Eigen中表示一个包含位移的3D变换,你需要将位移作为另一个独立的向量(例如`Eigen::Vector3d`)与矩阵组合起来,形成一个复合变换矩阵,比如`*R + translation`. 如果你想在`Matrix3d`上实现平移功能,通常会先创建一个单独的向量(如`Eigen::Translation3d`),然后将其转换为4x4的齐次矩阵并与`Matrix3d`相乘。在Eigen中有`Translation3f`或`Translation3d`这样的类专门用来表示这种三维平移。 ```cpp // 创建一个3D位移向量 Eigen::Vector3d translation(0, 0, 0); // x, y, z方向的位移 // 创建一个平移矩阵 Eigen::Translation3d t(translation); // 将平移矩阵与旋转矩阵结合 Eigen::Matrix4d combined = t.matrix() * R; ```
相关问题

Eigen::Matrix3d R1,R2。这两变量的坐标系是统一的,现将他们平移到同一坐标点怎么实现。C++实现

在C++中,如果你有两个`Eigen::Matrix3d`类型的变量`R1`和`R2`,它们代表两个三维旋转矩阵,但假设它们都是相对于各自独立的坐标系。如果你想将这两个旋转矩阵应用到同一个坐标点上,首先你需要明确这个公共的新坐标点。假设新坐标点为`(x_new, y_new, z_new)`。 为了将旋转应用到该点上,你需要做的是先将点从旧坐标系变换到新的坐标系,然后再进行旋转。这可以通过以下步骤实现: ```cpp // 新的坐标点 Eigen::Vector3d point_new(x_new, y_new, z_new); // 将点从旧坐标系移动到原点 Eigen::Vector3d point_in_old_coordinate = point_new - R1.translation(); // 如果R1的原点就是旧坐标系的原点,那么可以省略这一行 // 将点从原点变换到旋转后的点 Eigen::Vector3d rotated_point = R1 * point_in_old_coordinate; // 现在rotated_point已经在R1对应的坐标系下。如果需要将其转换到R2对应的坐标系,再次旋转 Eigen::Vector3d final_point = R2 * rotated_point; ``` 这里我们首先计算了点在`R1`旋转前的位置,然后对该位置进行了`R1`的旋转,最后再对结果进行`R2`的旋转。这就是在保持坐标一致性的前提下,如何使用`Eigen`库进行旋转和平移操作。

Eigen::Matrix4f sd 给sd赋值,表示(x,y,z)平移和z轴旋转π/8

Eigen库中的`Matrix4f`是一个四维矩阵,常用于三维空间变换。如果你想要对一个`sd`变量(假设它是一个`Eigen::Matrix4f`类型的矩阵)进行赋值,以表示从原点开始的(x, y, z)平移以及绕z轴旋转π/8弧度,可以按照以下步骤操作: 首先,你需要创建一个4x4单位矩阵(identity matrix),因为基础的旋转和平移矩阵会基于这个单位矩阵来构造。然后,分别处理平移和旋转部分。 ```cpp // 创建一个单位矩阵作为基础 Eigen::Matrix4f baseTransform(Eigen::Matrix4f::Identity()); // 平移部分 (x, y, z) Eigen::Vector3f translation(x, y, z); baseTransform.block<3, 3>(0, 3) = translation; // 将平移向量添加到最后一行 // 旋转部分,围绕z轴 π/8 弧度 double rotationRadians = M_PI / 8; Eigen::Quaternionf quat(rotationRadians, Eigen::Vector3f(0, 0, 1)); // 使用Z轴 Eigen::AngleAxisf aa(rotationRadians, Eigen::Vector3f(0, 0, 1)); // 或者使用AngleAxisf(更直接) baseTransform = baseTransform * quat.matrix(); // 应用旋转 // 最终赋值给sd sd = baseTransform; ``` 现在`sd`矩阵包含了所需的平移和旋转。
阅读全文

相关推荐

#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; }解释一下

大家在看

recommend-type

TXT文件合并器一款合并文本文件的工具

TXT文件合并器,一款合并文本文件的工具,可以的。
recommend-type

Scratch语言教程&案例&相关项目资源

这篇文章为想要学习和探索Scratch编程的青少年和初学者们提供了宝贵的教程、案例以及相关项目资源,旨在帮助他们轻松入门Scratch编程,并在实践中不断提升编程能力。 文章首先聚焦于Scratch教程的介绍,强调了教程在Scratch编程学习中的重要性。通过精心挑选的一系列优质教程资源,文章引导读者逐步了解Scratch的基本界面、积木块功能以及编程逻辑等核心概念。这些教程采用图文结合的方式,使得复杂的编程概念变得简单易懂,帮助初学者快速掌握Scratch编程的基础知识。 除了基础教程,文章还深入探讨了Scratch案例学习的价值。通过展示一系列真实而有趣的Scratch案例,文章让读者了解到Scratch在动画设计、游戏制作等领域的广泛应用。这些案例不仅具有创意和趣味性,而且能够帮助读者将所学知识应用到实际项目中,提升解决实际问题的能力。 此外,文章还梳理了与Scratch相关的项目资源,为学习者提供了实践Scratch编程的机会。这些项目资源包括Scratch社区分享的项目、学校或教育机构的实践项目等,为学习者提供了丰富的实战演练场景。通过参与这些项目,学习者不仅可以锻炼编
recommend-type

Xilinx 7系列FPGA手册[打包下载]

Xilinx 7系列FPGA手册打包下载,包括以下手册: 1)ug470_7Series_Config.pdf 2)ug471_7Series_SelectIO.pdf 3)ug472_7Series_Clocking.pdf 4)ug473_7Series_Memory_Resources.pdf 5)ug474_7Series_CLB.pdf 6)ug479_7Series_DSP48E1.pdf 7)ug480_7Series_XADC.pdf 8)ug482_7Series_GTP_Transceivers.pdf
recommend-type

filter LTC1068 模块AD设计 Altium设计 硬件原理图+PCB文件.rar

filter LTC1068 模块AD设计 Altium设计 硬件原理图+PCB文件,2层板设计,Altium Designer 设计的工程文件,包括完整的原理图及PCB文件,可以用Altium(AD)软件打开或修改,可作为你产品设计的参考。
recommend-type

谐响应分析步骤-ANSYS谐响应分析

谐响应分析 第三节:步骤 四个主要步骤: 建模 选择分析类型和选项 施加谐波载荷并求解 观看结果

最新推荐

recommend-type

2022年互联网金融行业分析报告.pptx

2022年互联网金融行业分析报告.pptx
recommend-type

广东省广电集团公司大客户电力负荷管理系统通信规约补充内容.doc

广东省广电集团公司大客户电力负荷管理系统通信规约补充内容.doc
recommend-type

单片机专业技能竞赛培训知识分享.ppt

单片机专业技能竞赛培训知识分享.ppt
recommend-type

吉林大学2021年9月《过程控制与自动化仪表》作业考核试题及答案参考14.docx

吉林大学2021年9月《过程控制与自动化仪表》作业考核试题及答案参考14.docx
recommend-type

医院档案信息化建设面临的问题与对策.docx

医院档案信息化建设面临的问题与对策.docx
recommend-type

全面解析SOAP库包功能与应用

从给定的文件信息中,我们可以提取到的核心知识点主要集中在“SOAP”这一项技术上,由于提供的信息量有限,这里将尽可能详细地解释SOAP相关的知识。 首先,SOAP代表简单对象访问协议(Simple Object Access Protocol),是一种基于XML的消息传递协议。它主要用于在网络上不同应用程序之间的通信。SOAP定义了如何通过HTTP和XML格式来构造消息,并规定了消息的格式应遵循XML模式。这种消息格式使得两个不同平台或不同编程语言的应用程序之间能够进行松耦合的服务交互。 在分布式计算环境中,SOAP作为一种中间件技术,可以被看作是应用程序之间的一种远程过程调用(RPC)机制。它通常与Web服务结合使用,Web服务是使用特定标准实现的软件系统,它公开了可以通过网络(通常是互联网)访问的API。当客户端与服务端通过SOAP进行通信时,客户端可以调用服务端上特定的方法,而不需要关心该服务是如何实现的,或者是运行在什么类型的服务器上。 SOAP协议的特点主要包括: 1. **平台无关性**:SOAP基于XML,XML是一种跨平台的标准化数据格式,因此SOAP能够跨越不同的操作系统和编程语言平台进行通信。 2. **HTTP协议绑定**:虽然SOAP协议本身独立于传输协议,但是它通常与HTTP协议绑定,这使得SOAP能够利用HTTP的普及性和无需额外配置的优势。 3. **消息模型**:SOAP消息是交换信息的载体,遵循严格的结构,包含三个主要部分:信封(Envelope)、标题(Header)和正文(Body)。信封是消息的外壳,定义了消息的开始和结束;标题可以包含各种可选属性,如安全性信息;正文则是实际的消息内容。 4. **错误处理**:SOAP提供了详细的错误处理机制,可以通过错误码和错误信息来描述消息处理过程中的错误情况。 5. **安全性和事务支持**:SOAP协议可以集成各种安全性标准,如WS-Security,以确保消息传输过程中的安全性和完整性。同时,SOAP消息可以包含事务信息,以便于服务端处理事务性的业务逻辑。 在描述中提到的“所有库包”,这可能意味着包含了SOAP协议的实现、相关工具集或库等。由于信息不足,这里的“库包”具体指的是什么并不清楚,但可以理解为与SOAP相关的软件开发工具包(SDK)或框架,它们使得开发者可以更加方便地创建SOAP消息,处理SOAP请求和响应,以及实现Web服务。 至于“压缩包子文件的文件名称列表”中只有一个单词“soap”,这可能表明实际文件内容仅有一个与SOAP相关的文件,或者是一个压缩包文件的名称为“soap”。由于缺乏更详尽的文件列表,无法进一步分析其可能的内容。 综上所述,SOAP作为一种实现Web服务的技术标准,通过HTTP和XML实现系统间的通信。它支持跨平台、跨语言的服务调用,并具备强大的安全性和错误处理机制。在具体应用中,开发者可能需要用到与之相关的库包或工具集来开发和部署Web服务。
recommend-type

编程语言选择指南:为不同项目量身定制的编程语言策略

# 摘要 选择合适的编程语言对于软件项目的成功至关重要。本文首先探讨了编程语言选择的重要性,并对不同分类和特点的编程语言进行了详尽分析,重点介绍了静态与动态类型、解释型与编译型、面向对象与函数式编程语言的区别和应用场景。接着,文章根据项目需求详细讨论了如何选择适合的编程语言,包括功能需求、性能考量、开发效率和团队技能等关键因素。通过实践案例分析,本文进一步探讨了不同
recommend-type

手写vue2的插件vue-router

<think>我们正在手写一个简化版的vue-router插件,用于Vue2。主要步骤:1.定义一个类VueRouter,包含路由映射、当前路由状态等。2.实现install方法,用于Vue.use()安装插件,在install方法中:-混入beforeCreate生命周期钩子,在组件创建时初始化router。-全局注册router-view和router-link组件。3.实现路由映射:将路由配置转换为路径与组件的映射表。4.监听hashchange事件(或者使用history模式,这里以hash模式为例)来响应路由变化。5.实现router-view组件,根据当前路由渲染对应组件。6.实现
recommend-type

《软件工程:实践者的方法》第6版课件解析

根据提供的文件信息,我们可以推断出以下知识点: 1. 课程名称:“SOFTWARE ENGINEERING A practitioner's approach 6e”,表明这是关于软件工程的课程教材,第6版,针对实践者的教学方法。 2. 版本信息:由于标题中明确指出是第6版(6e),我们知道这是一系列教科书或课件的最新版本,这意味着内容已经根据最新的软件工程理论和实践进行了更新和改进。 3. 课程类型:课程是针对“practitioner”,即实践者的,这表明教材旨在教授学生如何将理论知识应用于实际工作中,注重解决实际问题和案例学习,可能包含大量的项目管理、需求分析、系统设计和测试等方面的内容。 4. 适用范围:文件描述中提到了“仅供校园内使用”,说明这个教材是专为教育机构内部学习而设计的,可能含有某些版权保护的内容,不允许未经授权的外部使用。 5. 标签:“SOFTWARE ENGINEERING A practitioner's approach 6e 软件工程”提供了关于这门课程的直接标签信息。标签不仅重复了课程名称,还强化了这是关于软件工程的知识。软件工程作为一门学科,涉及软件开发的整个生命周期,从需求收集、设计、编码、测试到维护和退役,因此课程内容可能涵盖了这些方面。 6. 文件命名:压缩包文件名“SftEng”是“SOFTWARE ENGINEERING”的缩写,表明该压缩包包含的是软件工程相关的教材或资料。 7. 关键知识点:根据标题和描述,我们可以推测课件中可能包含的知识点有: - 软件工程基础理论:包括软件工程的定义、目标、原则和软件开发生命周期的模型。 - 需求分析:学习如何获取、分析、记录和管理软件需求。 - 系统设计:涉及软件架构设计、数据库设计、界面设计等,以及如何将需求转化为设计文档。 - 实现与编码:包括编程语言的选择、代码编写规范、版本控制等。 - 测试:软件测试的原则、方法和测试用例的设计。 - 项目管理:时间管理、团队协作、风险管理、成本估算等与软件项目成功相关的管理活动。 - 质量保证:软件质量模型、质量度量和质量控制措施。 - 维护和演化:软件部署后如何进行持续维护、升级和系统退役。 - 软件工程的新兴领域:包括敏捷开发方法、DevOps、用户体验设计等现代软件开发趋势。 8. 版权和使用限制:由于是专供校园内使用的教材,课件可能包含版权声明和使用限制,要求用户在没有授权的情况下不得对外传播和用于商业用途。 综上所述,这门课程的课件是为校园内的学生和教职员工设计的,关于软件工程的全面教育材料,覆盖了理论知识和实践技巧,并且在版权方面有所限制。由于是最新版的教材,它很可能包含了最新的软件工程技术和方法论。
recommend-type

QUARTUS II 13.0全攻略:新手到专家的10个必备技能

# 摘要 本文旨在详细介绍QUARTUS II 13.0软件的使用,包括其安装、FPGA基础、项目设置、设计流程、高级功能应用