ros tf2 如何输出相对角度信息

时间: 2023-08-11 16:05:31 浏览: 287
在ROS中,可以使用tf2来获取两个坐标系之间的相对变换信息,包括平移和旋转。如果你想要获取相对角度信息,可以使用tf2中的函数tf2::Matrix3x3::getRPY()。 具体步骤如下: 1.先使用tf2中的tfBuffer类创建一个tf2监听器,用于订阅TF变换信息。 2.在回调函数中,使用tf2::lookupTransform函数获取两个坐标系之间的变换信息,将其转换为旋转矩阵。 3.然后使用tf2::Matrix3x3::getRPY()函数,将旋转矩阵转换为欧拉角(roll、pitch、yaw),其中yaw角度即为两个坐标系之间的相对角度信息。 下面是一个示例代码: ``` #include <ros/ros.h> #include <tf2_ros/transform_listener.h> #include <tf2/LinearMath/Matrix3x3.h> int main(int argc, char** argv){ ros::init(argc, argv, "tf2_listener"); ros::NodeHandle node; tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); ros::Rate rate(10.0); while (node.ok()){ geometry_msgs::TransformStamped transformStamped; try{ transformStamped = tfBuffer.lookupTransform("target_frame", "source_frame", ros::Time(0)); tf2::Matrix3x3 rotation(transformStamped.transform.rotation); double roll, pitch, yaw; rotation.getRPY(roll, pitch, yaw); ROS_INFO("Relative yaw angle: %f", yaw); } catch (tf2::TransformException &ex) { ROS_WARN("%s",ex.what()); } rate.sleep(); } return 0; } ``` 在以上示例代码中,我们订阅了"target_frame"和"source_frame"两个坐标系之间的变换信息,并将其转换为旋转矩阵,然后使用getRPY函数获取yaw角度,输出相对角度信息。
阅读全文

相关推荐

#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

{团队建设}网络渠道部团队规划手册.pdf

{团队建设}网络渠道部团队规划手册.pdf
recommend-type

Wamp5: 一键配置ASP/PHP/HTML服务器工具

根据提供的文件信息,以下是关于标题、描述和文件列表中所涉及知识点的详细阐述。 ### 标题知识点 标题中提到的是"PHP集成版工具wamp5.rar",这里面包含了以下几个重要知识点: 1. **PHP**: PHP是一种广泛使用的开源服务器端脚本语言,主要用于网站开发。它可以嵌入到HTML中,从而让网页具有动态内容。PHP因其开源、跨平台、面向对象、安全性高等特点,成为最流行的网站开发语言之一。 2. **集成版工具**: 集成版工具通常指的是将多个功能组合在一起的软件包,目的是为了简化安装和配置流程。在PHP开发环境中,这样的集成工具通常包括了PHP解释器、Web服务器以及数据库管理系统等关键组件。 3. **Wamp5**: Wamp5是这类集成版工具的一种,它基于Windows操作系统。Wamp5的名称来源于它包含的主要组件的首字母缩写,即Windows、Apache、MySQL和PHP。这种工具允许开发者快速搭建本地Web开发环境,无需分别安装和配置各个组件。 4. **RAR压缩文件**: RAR是一种常见的文件压缩格式,它以较小的体积存储数据,便于传输和存储。RAR文件通常需要特定的解压缩软件进行解压缩操作。 ### 描述知识点 描述中提到了工具的一个重要功能:“可以自动配置asp/php/html等的服务器, 不用辛辛苦苦的为怎么配置服务器而烦恼”。这里面涵盖了以下知识点: 1. **自动配置**: 自动配置功能意味着该工具能够简化服务器的搭建过程,用户不需要手动进行繁琐的配置步骤,如修改配置文件、启动服务等。这是集成版工具的一项重要功能,极大地降低了初学者的技术门槛。 2. **ASP/PHP/HTML**: 这三种技术是Web开发中常用的组件。ASP (Active Server Pages) 是微软开发的服务器端脚本环境;HTML (HyperText Markup Language) 是用于创建网页的标准标记语言;PHP是服务器端脚本语言。在Wamp5这类集成环境中,可以很容易地对这些技术进行测试和开发,因为它们已经预配置在一起。 3. **服务器**: 在Web开发中,服务器是一个运行Web应用程序并响应客户端请求的软件或硬件系统。常见的服务器软件包括Apache、Nginx等。集成版工具提供了一个本地服务器环境,使得开发者可以在本地测试他们的应用程序。 ### 标签知识点 标签中仅出现了“PHP”一个关键词,这意味着该工具专注于与PHP相关的开发环境配置。 ### 压缩包子文件的文件名称列表知识点 1. **wamp.exe**: 这是Wamp5集成版工具的可执行文件,用户通过运行这个文件,即可启动Wamp5环境,开始进行PHP等相关开发。 2. **使用说明文档.txt**: 通常这样的文本文件包含了软件的安装指南、功能介绍、常见问题解答以及操作教程等内容。它是用户了解和使用软件的重要辅助资料,可以帮助用户快速上手。 ### 总结 Wamp5作为一个PHP集成版工具,为PHP开发者提供了一个便捷的本地开发环境。它通过自动化配置减少了开发者在搭建环境时的麻烦,整合了Web开发的关键组件,从而使得开发者能够专注于代码的编写,而不是环境配置。此外,它还提供了详尽的使用文档,帮助用户理解和运用这一工具,有效地提高了开发效率。对于希望快速入门PHP或需要一个简单易用的本地开发环境的开发者来说,Wamp5是一个非常好的选择。
recommend-type

【数据融合技术】:甘肃土壤类型空间分析中的专业性应用

# 摘要 数据融合技术作为一种集成多源数据信息的方法,在土壤类型空间分析中发挥着关键作用。本文介绍了数据融合技术的基本概念及其理论基础,阐述了数据预处理、同步整合及冲突解决等关键技术,并详细描述了甘肃土壤类型数据准备的流程,包括数据采集、质量评估、空间化处理及融合实践准备。通过具体案例分析,展示了数据融合在土壤类型空间分布分析、土壤质量评估及土壤保护规划中的应用。同时,文
recommend-type

sht20温湿度传感器使用什么将上拉电阻和滤波电容引出

<think>好的,我需要解决用户关于SHT20温湿度传感器的上拉电阻和滤波电容的引出方法问题。首先,我得回忆一下SHT20的基本特性和常见电路设计。SHT20是数字传感器,通常使用I2C接口,所以上拉电阻是必须的,用于确保数据线和时钟线的稳定。根据引用[2],SHT10是通过SCK和DATA线与单片机通信,而SHT30在引用[3]中使用I2C协议,需要上拉电阻。虽然用户问的是SHT20,但SHT系列通常设计类似,所以可以推断SHT20也需要类似的上拉电阻配置。通常I2C总线的上拉电阻值在4.7kΩ到10kΩ之间,但具体值可能取决于总线速度和电源电压。需要确认数据手册中的推荐值,但用户可能没有
recommend-type

Delphi仿速达财务软件导航条组件开发教程

Delphi作为一款历史悠久的集成开发环境(IDE),由Embarcadero Technologies公司开发,它使用Object Pascal语言,被广泛应用于Windows平台下的桌面应用程序开发。在Delphi中开发组件是一项核心技术,它允许开发者创建可复用的代码单元,提高开发效率和软件模块化水平。本文将详细介绍如何在Delphi环境下仿制速达财务软件中的导航条组件,这不仅涉及到组件的创建和使用,还会涉及界面设计和事件处理等技术点。 首先,需要了解Delphi组件的基本概念。在Delphi中,组件是一种特殊的对象,它们被放置在窗体(Form)上,可以响应用户操作并进行交互。组件可以是可视的,也可以是不可视的,可视组件在设计时就能在窗体上看到,如按钮、编辑框等;不可视组件则主要用于后台服务,如定时器、数据库连接等。组件的源码可以分为接口部分和实现部分,接口部分描述组件的属性和方法,实现部分包含方法的具体代码。 在开发仿速达财务软件的导航条组件时,我们需要关注以下几个方面的知识点: 1. 组件的继承体系 仿制组件首先需要确定继承体系。在Delphi中,大多数可视组件都继承自TControl或其子类,如TPanel、TButton等。导航条组件通常会继承自TPanel或者TWinControl,这取决于导航条是否需要支持子组件的放置。如果导航条只是单纯的一个显示区域,TPanel即可满足需求;如果导航条上有多个按钮或其他控件,可能需要继承自TWinControl以提供对子组件的支持。 2. 界面设计与绘制 组件的外观和交互是用户的第一印象。在Delphi中,可视组件的界面主要通过重写OnPaint事件来完成。Delphi提供了丰富的绘图工具,如Canvas对象,使用它可以绘制各种图形,如直线、矩形、椭圆等,并且可以对字体、颜色进行设置。对于导航条,可能需要绘制背景图案、分隔线条、选中状态的高亮等。 3. 事件处理 导航条组件需要响应用户的交互操作,例如鼠标点击事件。在Delphi中,可以通过重写组件的OnClick事件来响应用户的点击操作,进而实现导航条的导航功能。如果导航条上的项目较多,还可能需要考虑使用滚动条,让更多的导航项能够显示在窗体上。 4. 用户自定义属性和方法 为了使组件更加灵活和强大,开发者通常会为组件添加自定义的属性和方法。在导航条组件中,开发者可能会添加属性来定义按钮个数、按钮文本、按钮位置等;同时可能会添加方法来处理特定的事件,如自动调整按钮位置以适应不同的显示尺寸等。 5. 数据绑定和状态同步 在财务软件中,导航条往往需要与软件其他部分的状态进行同步。例如,用户当前所处的功能模块会影响导航条上相应项目的选中状态。这通常涉及到数据绑定技术,Delphi支持组件间的属性绑定,通过数据绑定可以轻松实现组件状态的同步。 6. 导航条组件的封装和发布 开发完毕后,组件需要被封装成独立的单元供其他项目使用。封装通常涉及将组件源码保存为pas文件,并在设计时能够在组件面板中找到。发布组件可能还需要编写相应的安装包和使用文档,方便其他开发者安装和使用。 7. Delphi IDE的支持 Delphi IDE提供了组件面板编辑器(Component Palette),允许开发者将开发好的组件添加到组件面板中。在组件面板编辑器中,可以自定义组件的图标和分类,使得组件在Delphi中的使用更为便捷。 通过以上的知识点梳理,可以看出Delphi仿速达导航条组件的开发涉及到的不仅仅是简单的代码编写,还涉及到用户界面设计、事件驱动编程、组件封装等多个方面。掌握这些知识点,对于一名Delphi开发者而言,是十分重要的。
recommend-type

【空间分布规律】:甘肃土壤类型与农业生产的关联性研究

# 摘要 本文对甘肃土壤类型及其在农业生产中的作用进行了系统性研究。首先概述了甘肃土壤类型的基础理论,并探讨了土壤类型与农业生产的理论联系。通过GIS技术分析,本文详细阐述了甘肃土壤的空间分布规律,并对其特征和影响因素进行了深入分析。此外,本文还研究了甘肃土壤类型对农业生产实际影响,包括不同区域土壤改良和作物种植案例,以及土壤养分、水分管理对作物生长周期和产量的具体影响。最后,提出了促进甘肃土壤与农业可持续发展的策略,包括土壤保护、退化防治对策以及土壤类型优化与农业创新的结合。本文旨在为
recommend-type

常见运放电路的基本结构和基本原理

<think>首先,用户的问题是关于运算放大器电路的基本结构和工作原理。我需要根据系统级指令来构建回答。系统级指令:-所有行内数学表达式必须使用$...$格式。-独立公式必须使用$$...$$格式并单独成段。-LaTeX语法正确。-使用中文回答。-生成相关问题。-回答中引用的段落末尾自然地添加引用标识,例如[^1]。-回答结构清晰,帮助用户逐步解决问题,尽量保证回答真实可靠。参考站内引用:-引用[1]:关于运算放大器基本电路用法,特别是反相放大器电路。-引用[2]:关于uA741运算放大器电路的基本原理,包括输入级、输出级等。用户的问题:"我想了解运放电路的基本结构和工作原理请问运算放大器电路
recommend-type

ASP.NET2.0初学者个人网站实例分享

标题:“ASP.NET 2.0个人网站”指向了一个网站开发项目,这个项目是使用ASP.NET 2.0框架构建的。ASP.NET 2.0是微软公司推出的一种用于Web开发的服务器端技术,它是.NET Framework的一部分。这个框架允许开发者构建动态网站、网络应用程序和网络服务。开发者可以使用C#或VB.NET等编程语言来编写应用程序。由于这被标签为“2.0”,我们可以假设这是一个较早版本的ASP.NET,相较于后来的版本,它可能没有那么先进的特性,但对于初学者来说,它提供了基础并且易于上手的工具和控件来学习Web开发。 描述:“个人练习所做,适合ASP.NET初学者参考啊,有兴趣的可以前来下载去看看,同时帮小弟我赚些积分”提供了关于该项目的背景信息。它是某个个人开发者或学习者为了实践和学习ASP.NET 2.0而创建的个人网站项目。这个项目被描述为适合初学者作为学习参考。开发者可能是为了积累积分或网络声誉,鼓励他人下载该项目。这样的描述说明了该项目可以被其他人获取,进行学习和参考,或许还能给予原作者一些社区积分或其他形式的回报。 标签:“2.0”表明这个项目专门针对ASP.NET的2.0版本,可能意味着它不是最新的项目,但是它可以帮助初学者理解早期ASP.NET版本的设计和开发模式。这个标签对于那些寻找具体版本教程或资料的人来说是有用的。 压缩包子文件的文件名称列表:“MySelf”表示在分享的压缩文件中,可能包含了与“ASP.NET 2.0个人网站”项目相关的所有文件。文件名“我的”是中文,可能是指创建者以“我”为中心构建了这个个人网站。虽然文件名本身没有提供太多的信息,但我们可以推测它包含的是网站源代码、相关资源文件、数据库文件(如果有的话)、配置文件和可能的文档说明等。 知识点总结: 1. ASP.NET 2.0是.NET Framework下的一个用于构建Web应用程序的服务器端框架。 2. 它支持使用C#和VB.NET等.NET支持的编程语言进行开发。 3. ASP.NET 2.0提供了一组丰富的控件,可帮助开发者快速构建Web表单、用户界面以及实现后台逻辑。 4. 它还提供了一种称作“Web站点”项目模板,使得初学者能够方便地开始Web开发项目。 5. ASP.NET 2.0是微软.NET历史上一个重要的里程碑,引入了许多创新特性,如成员资格和角色管理、主题和皮肤、网站导航和个性化设置等。 6. 在学习ASP.NET 2.0的过程中,初学者可以了解到如HTTP请求和响应、服务器控件、状态管理、数据绑定、缓存策略等基础概念。 7. 本项目可作为ASP.NET初学者的实践平台,帮助他们理解框架的基本结构和工作流程,从而为学习更高版本的ASP.NET打下坚实基础。 8. 个人网站项目的构建可以涵盖前端设计(HTML, CSS, JavaScript)和后端逻辑(C#或VB.NET)的综合应用。 9. 在学习过程中,初学者应该学会如何配置和使用IIS(Internet Information Services)来部署ASP.NET网站。 10. “赚取积分”可能指的是在某个在线社区、论坛或代码托管平台上,通过分享项目来获得一定的积分或奖励,这通常是用来衡量用户对社区贡献大小的一种方式。 综上所述,该“ASP.NET 2.0个人网站”项目不仅为初学者提供了一个实用的学习资源,同时体现了开发者对于开源共享精神的实践,对社区贡献出自己的力量。通过这样的实践,初学者能够更好地理解ASP.NET框架的运作,逐步建立起自己的Web开发技能。
recommend-type

【制图技术】:甘肃高质量土壤分布TIF图件的成图策略

# 摘要 本文针对甘肃土壤分布数据的TIF图件制作进行了系统研究。首先概述了甘肃土壤的分布情况,接着介绍了TIF图件的基础知识,包括其格式特点、空间数据表达以及质量控制方法。随后,文中构建了成图策略的理论框架,分析了土壤分布图的信息需求与数据处理流程,并探讨了成图原则与标准。在实践操作部分,详细阐述了制图软
recommend-type

代码解释 ```c char* image_data = (char*)malloc(width * height * channels); ```

<think>我们正在分析C语言中的一行代码:char* image_data = (char*)malloc(width * height * channels); 这行代码位于一个完整的代码片段中,该片段还包括检查内存分配是否成功。 代码功能分析: 这行代码的主要功能是动态分配一块内存,并将分配的内存地址赋值给指针变量image_data。 详细解析: 1. malloc函数:malloc是C语言中用于动态内存分配的函数,它从堆上分配指定字节数的内存,并返回一个指向这块内存起始地址的void指针(void*)。如果分配失败,则返回NULL。 2. 参数:malloc的参数