std::tuple<int, float, std::string> data;data插入初始元素
时间: 2024-10-15 14:12:52 浏览: 59
在C++中,`std::tuple` 是一种固定大小的、不可变的联合容器,用于存储不同类型的数据。如果你想给 `std::tuple` 插入初始元素,你需要在创建 tuple 的时候就指定这些元素。例如:
```cpp
#include <iostream>
#include <tuple>
int main() {
std::tuple<int, float, std::string> data(10, 3.14f, "Initial Value"); // 创建并初始化 tuple
std::cout << "Tuple elements: ";
std::get<0>(data) << ", " << std::get<1>(data) << ", " << std::get<2>(data) << std::endl;
return 0;
}
```
在这个例子中,我们在创建 tuple `data` 时直接传入了三个参数(10, 3.14f, "Initial Value"),它们分别对应 `std::tuple<int, float, std::string>` 中的三个位置。
相关问题
#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; }解释一下
### 图像处理与坐标转换模块
该代码段首先通过 OpenCV 处理图像数据,检测目标物体的像素坐标。通常使用颜色阈值分割或边缘检测等方法提取目标特征。例如,以下代码展示了如何将图像从 BGR 转换为 HSV 颜色空间,并对特定颜色范围进行掩码处理:
```cpp
cv::Mat hsvImage;
cv::cvtColor(rawImage, hsvImage, cv::COLOR_BGR2HSV);
cv::Scalar lowerBound = cv::Scalar(0, 100, 100);
cv::Scalar upperBound = cv::Scalar(10, 255, 255);
cv::Mat mask;
cv::inRange(hsvImage, lowerBound, upperBound, mask);
```
在完成图像预处理后,通过轮廓检测获取目标物体的位置信息。OpenCV 提供了 `findContours` 函数用于查找图像中的轮廓,并计算其质心作为目标的像素坐标 [^1]。
```cpp
std::vector<std::vector<cv::Point>> contours;
cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
if (!contours.empty()) {
auto& contour = contours[0];
double area = cv::contourArea(contour);
if (area > 100) {
cv::Moments m = cv::moments(contour);
int cx = static_cast<int>(m.m10 / m.m00);
int cy = static_cast<int>(m.m01 / m.m00);
pixelPoint = cv::Point2d(cx, cy);
}
}
```
### 相机到机械臂的坐标转换
为了将像素坐标转换为机械臂可识别的空间坐标,需要利用相机内参矩阵 K 和畸变参数 distCoeffs。OpenCV 提供了 `undistortPoints` 函数用于去除像素坐标的畸变影响,并结合相机外参(旋转和平移向量)将其转换为世界坐标系下的三维坐标 [^1]。
```cpp
cv::Mat cameraMatrix = (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);
std::vector<cv::Point2f> distortedPoints(1, pixelPoint);
std::vector<cv::Point2f> undistortedPoints;
cv::undistortPoints(distortedPoints, undistortedPoints, cameraMatrix, distCoeffs);
```
在获得去畸变后的归一化坐标后,还需结合深度信息(如来自 Kinect v2 的深度图)计算目标点的三维坐标。假设已知目标点的深度值 `depthValue`,则可通过以下方式计算其在相机坐标系下的位置:
```cpp
double x = (undistortedPoints[0].x * depthValue);
double y = (undistortedPoints[0].y * depthValue);
double z = depthValue;
Eigen::Vector4d cameraPoint(x, y, z, 1.0);
```
由于相机和机械臂之间的相对位姿是已知的(通过手眼标定获取),可以将相机坐标系下的点转换为机械臂基座标系下的坐标。ROS 中常用 TF 或 Eigen 库实现坐标变换:
```cpp
Eigen::Isometry3d transform; // 从相机到机械臂的变换矩阵
Eigen::Vector4d basePoint = transform * cameraPoint;
```
### MoveIt 控制机械臂运动
MoveIt 是 ROS 中用于机械臂路径规划和运动控制的核心库。在获得目标点的世界坐标后,可以通过 MoveIt 设置机械臂末端执行器的目标位姿并执行运动规划 [^1]。
```cpp
moveit::planning_interface::MoveGroupInterface move_group("arm");
geometry_msgs::Pose target_pose;
target_pose.orientation.w = 1.0;
target_pose.position.x = basePoint.x();
target_pose.position.y = basePoint.y();
target_pose.position.z = basePoint.z();
move_group.setPoseTarget(target_pose);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
if (success) {
move_group.execute(my_plan);
}
```
此外,代码中可能还包含对夹爪的控制逻辑,例如在接近目标点后闭合夹爪以抓取物体。ROS 提供了多种方式控制夹爪,包括直接发布关节控制指令或调用服务接口:
```cpp
std_msgs::Float64 gripper_cmd;
gripper_cmd.data = 0.0; // 关闭夹爪
gripper_pub.publish(gripper_cmd);
```
### 综合功能说明
该 C++ 程序实现了从图像采集、目标检测、坐标转换到机械臂运动控制的完整流程。整个系统依赖于多个关键组件的协同工作:
- **OpenCV**:负责图像预处理、目标识别及像素坐标校正。
- **ROS**:提供传感器数据订阅、TF 坐标变换及机械臂控制接口。
- **MoveIt**:实现机械臂的轨迹规划与运动控制。
- **xArm SDK**:与硬件通信,确保命令正确执行。
程序的整体架构体现了机器人视觉伺服系统的典型设计模式,即通过感知环境信息指导机械臂完成自主操作任务。
---
#include <GL/glut.h> #include <opencv2/opencv.hpp> #include <vector> #include <opencv2/core/matx.hpp> struct Vertex { float x, y, z; }; std::vector<std::vector<Vertex>> triangles; // 存储STL三角形数据 // STL文件加载函数(二进制格式) void LoadSTL(const char* filename) { FILE* file = fopen(filename, "rb"); if (!file) return; char header[80]; fread(header, 80, 1, file); uint32_t triCount; fread(&triCount, 4, 1, file); triangles.resize(triCount); for (auto& tri : triangles) { float normal[3]; fread(normal, 12, 1, file); tri.resize(3); fread(&tri[0].x, 12, 3, file); fseek(file, 2, SEEK_CUR); // 跳过属性字节 } fclose(file); } void Render() { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glBegin(GL_TRIANGLES); for (const auto& tri : triangles) { for (const auto& v : tri) { glVertex3f(v.x, v.y, v.z); } } glEnd(); glutSwapBuffers(); } void SaveImage(const char* filename) { int width = glutGet(GLUT_WINDOW_WIDTH); int height = glutGet(GLUT_WINDOW_HEIGHT); std::vector<uint8_t> pixels(3 * width * height); glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, pixels.data()); cv::Mat img(height, width, CV_8UC3, pixels.data()); cv::flip(img, img, 0); // OpenGL坐标系翻转 cv::cvtColor(img, img, cv::COLOR_RGB2BGR); cv::imwrite(filename, img); } void SetupView(glm::vec3 eye, glm::vec3 center, glm::vec3 up) { glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(45, 1.0, 0.1, 1000.0); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(eye.x, eye.y, eye.z, center.x, center.y, center.z, up.x, up.y, up.z); } int main(int argc, char** argv) { glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); glutInitWindowSize(800, 800); glutCreateWindow("STL Viewer"); glEnable(GL_DEPTH_TEST); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); LoadSTL("model.stl"); const std::vector<std::tuple<glm::vec3, glm::vec3, glm::vec3>> views = { // 前、后、左、右、顶、底视图参数 {{0,0,100}, {0,0,0}, {0,1,0}}, // Front {{0,0,-100}, {0,0,0}, {0,1,0}}, // Back {{100,0,0}, {0,0,0}, {0,1,0}}, // Left {{-100,0,0}, {0,0,0}, {0,1,0}}, // Right {{0,100,0}, {0,0,0}, {0,0,1}}, // Top {{0,-100,0}, {0,0,0}, {0,0,-1}} // Bottom }; for (int i = 0; i < views.size(); ++i) { const auto&[eye, center, up] = views[i]; SetupView(eye, center, up); Render(); SaveImage(("view_" + std::to_string(i) + ".jpg").c_str()); } return 0; } 这段代码报未定义标识符eye,, center, up
<think>好的,我现在需要帮助用户解决他们提供的C++代码中的错误。用户提到代码报未定义标识符eye、center、up。让我仔细看一下代码。
首先,用户提供的代码中,main函数里有一个views的vector,里面存储了多个tuple,每个tuple包含三个glm::vec3参数。然后在循环中,使用结构化绑定来解包这些参数:
const auto&[eye, center, up] = views[i];
但是用户说编译器报告eye、center、up未定义。可能的原因有几个:
1. 编译器不支持C++17的结构化绑定:结构化绑定是C++17引入的特性。如果用户使用的编译器版本较低,或者没有启用C++17模式,就会导致这个错误。
2. 缺少glm库的头文件:代码中使用了glm::vec3,如果没有包含正确的头文件,比如#include <glm/glm.hpp>,那么glm的vec3类型就无法识别,进而导致编译器无法解析tuple中的元素,可能间接导致结构化绑定的失败。
3. 其他可能的语法错误:比如views的初始化是否正确,或者tuple的模板参数是否正确。
接下来,我需要检查用户的代码中是否存在这些问题。首先看代码中的头文件部分,用户没有包含glm的头文件,所以在使用glm::vec3时肯定会出错。其次,代码中的结构化绑定是否正确使用了C++17。
解决方案应该是:
1. 包含glm的头文件,例如#include <glm/glm.hpp>。
2. 确保编译器支持C++17,并在编译时启用该标准。例如,使用GCC时需要添加-std=c++17选项。
3. 如果编译器不支持C++17,可以将结构化绑定改为使用std::get来获取tuple的元素。
此外,用户可能在代码中没有正确使用glm的命名空间,或者没有正确安装glm库,导致无法找到vec3的定义。这也是需要检查的一点。
另外,在代码中使用了glm::vec3作为tuple的元素类型,需要确认glm库是否正确安装,并且头文件路径被正确包含到项目中。
总结,用户的问题主要是两个:缺少glm头文件导致未定义的glm::vec3,以及结构化绑定需要C++17支持。因此,解决方案是添加头文件,并确保启用C++17标准。
最后,提醒用户检查编译命令是否正确,是否链接了必要的库,比如GLUT、OpenGL、OpenCV等,但这些问题可能不直接导致当前的错误。</think><<
该代码报错的主要原因是缺少`glm`命名空间的定义和C++17结构化绑定的支持问题。以下是具体分析和解决方案:
1. **缺少头文件包含**:
代码使用了`glm::vec3`但未包含GLM数学库的头文件
2. **结构化绑定需要C++17**:
`const auto&[eye, center, up]`语法属于C++17特性
**修改后的代码**:
```cpp
// 添加glm头文件
#include <glm/glm.hpp> // 必须包含GLM核心库
#include <GL/glut.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <tuple> // 需要包含tuple头文件
// ...其他代码保持不变...
int main(int argc, char** argv) {
// ...初始化代码...
// 修改为C++11兼容写法(如果编译器不支持C++17)
for (size_t i = 0; i < views.size(); ++i) {
const auto& view = views[i];
const glm::vec3& eye = std::get<0>(view);
const glm::vec3& center = std::get<1>(view);
const glm::vec3& up = std::get<2>(view);
SetupView(eye, center, up);
Render();
SaveImage(("view_" + std::to_string(i) + ".jpg").c_str());
}
return 0;
}
```
**编译建议**:
```bash
# 需要添加C++17编译选项
g++ -std=c++17 your_code.cpp -lglut -lGL -lGLU -lopencv_core -lopencv_imgcodecs
```
**其他可能需要添加的依赖**:
1. 安装GLM数学库(通常只需头文件)
2. 确保OpenGL、GLUT和OpenCV开发库已正确安装
3. 检查头文件包含路径是否正确
**完整修正方案**:
1. 安装GLM:`sudo apt-get install libglm-dev`
2. 在代码开头添加:`#include <glm/glm.hpp>`
3. 添加编译选项:`-std=c++17`
4. 使用tuple需要包含`#include <tuple>`
>>
阅读全文
相关推荐
















