配置环境及需要的包
- 工控机系统为ubuntu14.04及对应的ros版本
- 需要的安装包：
建议安装编译顺序：
kinova-ros-master------>ddynamic_reconfigure-kinetic-devel----->aruco_ros, iai_kinect2, usb_cam, image_pipeline-melodic, xfei_asr------->easy_handeye------>realsense-ros-------->phone-------->kinova_demo_paragram（请按照上述顺序编译安装，否则可能出现依赖包不满足，无法成功编译，同时忽略kinetic&Melodic关键词，安装时，搜索当前电脑的ros版本安装即可）
程序解读及流程图
- 预先启动项
启动kinova 机械臂
roslaunch kinova_bringup kinova_robot.launch
启动realsense2 D435i
roslaunch realsense2_camera rs_rgbd.launch
(注：这里面的color width&height 分别改为1280 & 720)
启动pad与工控机的ros通讯
rosrun phone phone
启动运行代码
rosrun kinova_demo_paragram WheelChair - 程序执行流程图
- 代码
#include "../include/ArmPart.h"
#include <ros/ros.h> //ros标准库头文件
#include <iostream> //C++标准输入输出库
#include <fstream>
#include <cmath>
#include <float.h>
#include <string>
#include <sstream>
#include <time.h>
/*
cv_bridge中包含CvBridge库
*/
#include <cv_bridge/cv_bridge.h>
/*
ROS图象类型的编码函数
*/
#include <sensor_msgs/image_encodings.h>
/*
image_transport 头文件用来在ROS系统中的话题上发布和订阅图象消息
*/
#include <image_transport/image_transport.h>
//OpenCV2标准头文件
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
// Kinova Header
#include <vector>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include <kinova_msgs/ArmPoseActionGoal.h> // 注意格式源文件名字为ArmPose.action
#include <kinova_driver/kinova_ros_types.h>
#include <actionlib/client/simple_action_client.h>
#include <kinova_msgs/SetFingersPositionAction.h>
#include <kinova_msgs/ArmPoseAction.h>
#include <kinova_msgs/HomeArm.h>
#include <kinova_msgs/ArmJointAnglesAction.h>
using namespace cv;
using namespace std;
std::vector<double> PICTURE_POSE = {-0.00991799216717, -0.319072604179, 0.403790742159, 0.993079960346, -0.0494419597089, 0.10155480355, 0.0321599170566};
std::vector<float> PICTURE_FINGER = {1, 1, 0};
std::vector<double> PICTURE_KPOSE = {0.32190772891, -0.0196264386177, 0.507371723652, -0.462231785059, -0.551994204521, -0.609356701374, -0.332157403231};
std::vector<float> PICTURE_KFINGER = {0, 0, 0};
std::vector<float> HOME_JOINT_COMMAND = {275, 167, 57, -119, 82, 75};
std::vector<float> KinectPhoto_JOINT_COMMAND = {240, 167, 57, -119, 82, 75};
float CATCH_CUP_DOWN_HEIGHT = 0.0075;
float TEASPOON_DOWN_HEIGHT = 0.005;
float LID_DOWN_HEIGHT = 0.002;
// All the object down or up height
const float cup_down = -0.00120655400679; // water cup lay down height
// First Cali 2019-12-25
//Mat A = (Mat_<float>(8, 3) << 673, 116, 1,
// 355, 228, 1,
// 990, 213, 1,
// 503, 350, 1,
// 848, 345, 1,
// 359, 481, 1,
// 681, 477, 1,
// 1002, 467, 1); // 7x3
//Mat B = (Mat_<float>(8, 3) << 0.431220471859, -0.358827114105, 1,
// 0.3800740242, -0.18310624361, 1,
// 0.370245963335, -0.541783392429, 1,
// 0.307792246342, -0.262980282307, 1,
// 0.300244480371, -0.451115190983, 1,
// 0.235510542989, -0.180042237043, 1,
// 0.226261809468, -0.361529707909, 1,
// 0.224617779255, -0.539018988609, 1); // 7x3
Mat A = (Mat_<float>(7, 3) << 653, 136, 1,
947, 236, 1,
464, 366, 1,
807, 366, 1,
315, 496, 1,
640, 495, 1,
956, 486, 1); // 7x3
Mat B = (Mat_<float>(7, 3) << 0.431220471859, -0.368827114105, 1,
0.370245963335, -0.541783392429, 1,
0.307792246342, -0.262980282307, 1,
0.300244480371, -0.461115190983, 1,
0.235510542989, -0.180042237043, 1,
0.226261809468, -0.361529707909, 1,
0.224617779255, -0.539018988609, 1); // 7x3
Mat X;
float kx, ky;
std::vector<float> finger_pos_command(3, 0);
std::vector<double> arm_pose_command(7, 0);
std::vector<float> arm_Joint_command(6, 0);
std::vector<double> HALF_POUR_pose_command = {-0.437474787235, 0.0819809064269, 0.0862290412188, -0.998309969902, 0.054526142776, 0.00104312982876, 0.0200746860355};
std::vector<double> FULL_POUR_pose_command = {0};
// Define for the Camera Feedback Results
const int num_x = 9; // number of divisions
const int num_y = 3; // number of quarters
float results[num_x][num_y], pixel[num_x][num_y];
//std::vector<cv::Point3i> Final_Centers;
//std::vector< std::vector<cv::Point3i>> Final_Centers;
std::vector<cv::Point3i> Final_Centers;
bool CAMERA_CB_FLAG = false;
/*
轮椅小桌板物体定位与识别 类
*/
class lunyi
{
private:
const int BINARY_VALUE = 77; //灰度二值化阈值（重要可调）
const int MinArea = 777; //允许存在的轮廓最小面积阈值（重要可调）
cv::Mat image, gray, colormask; //原图及对应灰度图
std::vector<std::vector<cv::Point> >workpieceContours; //轮廓
std::vector<cv::Point2d>workpieceBarycenters; //重心
std::vector<cv::Point2d>workpieceCenters; //中心
cv::CascadeClassifier faceCas;
public:
lunyi() {}
lunyi(bool load)
{
const char *faceCasName = "D:\\face.xml";
try
{
faceCas.load(faceCasName);
}
catch (cv::Exception e) {}
if (faceCas.empty())
{
std::cerr << "无法加载人脸级联分类器！" << std::endl;
std::exit(1);
}
}
void inputimg(const cv::Mat &img) { img.copyTo(image); } //类输入-输入必须是三通道图
std::vector<cv::Point2d>&getBarycenters() { return workpieceBarycenters; } // 返回重心
//二值化
inline void binaryzate(const cv::Mat &img, cv::Mat &out);
inline void binaryzate_hsv(const cv::Mat &img, char color, cv::Mat &out);
inline void binaryzate_yuv(const cv::Mat &img, char color, cv::Mat &out);
//轮廓查找及重心
size_t featureExtracte(const cv::Mat &binary,
std::vector<std::vector<cv::Point> >&workpieceContours,
std::vector<cv::Point2d>&workpieceBarycenters,
int mode = CV_RETR_EXTERNAL);
//结果绘制
void draw(cv::Mat &img, std::vector<std::vector<cv::Point> >workpieceContours,
std::vector<cv::Point2d>workpieceBarycenters);
//人脸检测和嘴巴定位
cv::Point2i face_mouth(const cv::Mat& img);
//定位流程（旧）
int workpiece_locate(const cv::Mat &img, int mode = 'B');
//定位流程（新，轮椅结题）
std::vector<cv::Point3i>& lunyi_jieti(const cv::Mat& img, std::string mode="ROYGBIP");
//定位流程（新，轮椅结题，随机定位）
std::vector<cv::Point3i> lunyi_jieti(const cv::Mat& img, char mode);
};
//内联空类型函数 - 基于灰度值的图像二值化
inline void lunyi::binaryzate(const cv::Mat &img, cv::Mat &out)
{
cv::Mat gray;
if (img.channels() == 3)
{
cv::cvtColor(img, gray, CV_BGR2GRAY);
}
else gray = img;
//高斯滤波去除高斯白噪声
GaussianBlur(gray, gray, cv::Size(7, 7), 1.5);
//二值化
cv::threshold(gray, out, BINARY_VALUE, 255, cv::THRESH_BINARY);
#define KSIZE 11
//中值滤波去噪
cv::medianBlur(out, out, KSIZE);
//形态学处理去噪
//cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(KSIZE, KSIZE));
//cv::dilate(out, out, element);
//cv::erode(out, out, element);
#undef KSIZE
//cv::imshow("二值化", out);
}
//内联空类型函数 - 基于HSV颜色空间的图像二值化
inline void lunyi::binaryzate_hsv(const cv::Mat &img, char color, cv::Mat &out)
{
cv::Mat img_hsv, mask;
//cv::GaussianBlur(img, img, cv::Size(7, 7), 1.2);
cv::cvtColor(img, img_hsv, cv::COLOR_BGR2HSV);
switch (color)
{//检查每个元素是否在给定数值之间
case 'R': // 红 西瓜
cv::inRange(img_hsv, cv::Scalar(0, 100, 133), cv::Scalar(10, 233, 255), mask);
break;
case 'O': // 橙 哈密瓜
cv::inRange(img_hsv, cv::Scalar(10, 66, 177), cv::Scalar(27, 200, 255), mask);
break;
case 'Y': // 黄 甜瓜
cv::inRange(img_hsv, cv::Scalar(13, 5, 177), cv::Scalar(40, 88, 255), mask);
break;
case 'G': // 绿
cv::inRange(img_hsv, cv::Scalar(40, 140, 128), cv::Scalar(55, 230, 256), mask);
break;
case 'B': // 蓝 脉动
cv::inRange(img_hsv, cv::Scalar(90, 120, 177), cv::Scalar(110, 255, 255), mask);
break;
case 'I': // 靛
cv::inRange(img_hsv, cv::Scalar(110, 140, 128), cv::Scalar(150, 230, 256), mask);
break;
case 'P': // 紫
cv::inRange(img_hsv, cv::Scalar(150, 70, 128), cv::Scalar(166, 180, 256), mask);
break;
default: std::cerr << "mode传参有错" << std::endl;
}
#define KSIZE 13
//中值滤波去噪声
cv::medianBlur(mask, out, KSIZE);
//形态学处理去噪
//cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(KSIZE, KSIZE));
//cv::dilate(out, out, element);
//cv::erode(out, out, element);
#undef KSIZE
img.copyTo(colormask, mask);
//img.copyTo(colormask);
}
//内联空类型函数 - 基于YCrCb颜色空间的图像二值化
inline void lunyi::binaryzate_yuv(const cv::Mat &img, char color, cv::Mat &out)
{
cv::Mat img_yuv, mask;
cv::GaussianBlur(img, img, cv::Size(7, 7), 1.5);
cv::cvtColor(img, img_yuv, cv::COLOR_BGR2YCrCb);
switch (color)
{//检查每个元素是否在给定数值之间
case 'B': // 蓝色
cv::inRange(img_yuv, cv::Scalar(30, 80, 160), cv::Scalar(130, 110, 190), mask);
break;
default: std::cerr << "mode传参有错" << std::endl;
}
#define KSIZE 15
//中值滤波去噪声
cv::medianBlur(mask, out, KSIZE);
//形态学处理去噪
//cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(KSIZE, KSIZE));
//cv::dilate(mask, out, element);
//cv::erode(out, out, element);
#undef KSIZE
//img.copyTo(colormask, mask);
img.copyTo(colormask);
}
//长整型函数 - 轮廓查找及重心计算
size_t lunyi::featureExtracte(const cv::Mat &binary,
std::vector<std::vector<cv::Point> >&workpieceContours,
std::vector<cv::Point2d>&workpieceBarycenters, int mode)
{
workpieceContours.erase(workpieceContours.begin(), workpieceContours.end());
workpieceBarycenters.erase(workpieceBarycenters.begin(), workpieceBarycenters.end());
std::vector<std::vector<cv::Point> >contours;
std::vector<cv::Vec4i>hierarchy;
cv::Mat binary_temp = binary.clone();
//轮廓查找
#if isWindows
gd::findContours3(binary_temp, contours, hierarchy, mode, CV_CHAIN_APPROX_NONE);
#else
cv::findContours(binary_temp, contours, hierarchy, mode, CV_CHAIN_APPROX_NONE);
#endif
//std::cout << "共有" << contours.size() << "个轮廓" << std::endl;
//筛掉面积较小的轮廓
std::vector<std::vector<cv::Point> >::iterator it = contours.begin();
for (int area = 0; it != contours.end(); ++it)
{
if ((area = static_cast<int>(cv::contourArea(cv::Mat(*it)))) > MinArea)
{
workpieceContours.push_back(*it);
}
}
//std::cout << "共有" << workpieceContours.size() << "个合格轮廓" << std::endl;
//用图像矩计算每个轮廓的重心
for (int i = 0; i < workpieceContours.size(); ++i)
{
cv::Moments moment = moments(cv::Mat(workpieceContours[i]));
cv::Point2d Barycenter(moment.m10 / moment.m00, moment.m01 / moment.m00);
workpieceBarycenters.push_back(Barycenter);
}
return workpieceContours.size();
}
//绘图
void lunyi::draw(cv::Mat &img,
std::vector<std::vector<cv::Point> >workpieceContours,
std::vector<cv::Point2d>workpieceBarycenters)
{
for (int i = 0; i < workpieceContours.size(); ++i)
{
cv::drawContours(img, workpieceContours, i, cv::Scalar(0, 255, 0), 2, 8);
cv::circle(img, workpieceBarycenters[i], 3, cv::Scalar(0, 0, 255), CV_FILLED);
//cv::circle(img, workpieceCenters[i], 3, cv::Scalar(255, 0, 0), CV_FILLED);
//cv::ellipse(img, cv::minAreaRect(workpieceContours[i]), cv::Scalar(255, 0, 0), 2);
}
cv::imshow("img", img);
}
//定位
int lunyi::workpiece_locate(const cv::Mat& img, int mode)
{
cv::Mat binaryImg;
binaryzate_hsv(img, mode, binaryImg);
featureExtracte(binaryImg, workpieceContours, workpieceBarycenters);
draw(colormask, workpieceContours, workpieceBarycenters);
return 0;
}
//轮椅结题嘴巴定位
cv::Point2i lunyi::face_mouth(const cv::Mat& img)
{
std::vector<cv::Rect>boxs;
cv::Rect maxBox(0, 0, 0, 0);
cv::Mat gray;
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
faceCas.detectMultiScale(gray, boxs, 1.1, 3, CV_HAAR_SCALE_IMAGE, cv::Size(32, 32));
if (boxs.size() > 0)
{
for (std::vector<cv::Rect>::const_iterator it_box = boxs.begin(); it_box != boxs.end(); ++it_box)
{
if (it_box->area() > maxBox.area())
maxBox = *it_box;
}
}
cv::Point2i mouth = cv::Point2i(static_cast<int>(maxBox.x + maxBox.width*0.5),
static_cast<int>(maxBox.y + maxBox.height*0.777));
std::cout << "嘴巴 坐标(如果没有检测到为0,0)：(" << mouth.x << ", " << mouth.y << "）" << std::endl;
return mouth;
}
//定位
std::vector<cv::Point3i>& lunyi::lunyi_jieti(const cv::Mat& img, std::string mode)
{
cv::Mat binaryImg;
std::vector<std::vector<cv::Point> >contours, Contours;
std::vector<cv::Point2d>barycenters, Barycenters;
std::vector<cv::Point3i>ResCenters;
for (int i = 0; i < mode.size(); ++i)
{
binaryzate_hsv(img, mode[i], binaryImg);
featureExtracte(binaryImg, Contours, Barycenters);
contours.insert(contours.end(), Contours.begin(), Contours.end());
barycenters.insert(barycenters.end(), Barycenters.begin(), Barycenters.end());
for (std::vector<cv::Point2d>::iterator it = Barycenters.begin(); it != Barycenters.end(); ++it)
{
if (it->x < 170 || it->x>1120 || it->y < 50 || it->y>580) continue; //仅返回小桌板处的水果
ResCenters.push_back(cv::Point3i(static_cast<int>(it->x), static_cast<int>(it->y), mode[i]));
}
}
draw(colormask, contours, barycenters);
for (std::vector<cv::Point3i>::iterator it = ResCenters.begin(); it != ResCenters.end(); ++it)
{
std::cout << "类别：" << it->z << ", 坐标：(" << it->x << ", " << it->y << "）" << std::endl;
}
return ResCenters;
}
//随机定位
std::vector<cv::Point3i> lunyi::lunyi_jieti(const cv::Mat& img, char mode)
{
const int radius = 22;
cv::Mat binaryImg, im = img.clone();
std::vector<cv::Point3i>ResCenters;
binaryzate_hsv(img, mode, binaryImg);
if (cv::countNonZero(binaryImg) < 2500)
{
std::cerr << "该颜色的水果已经拿完或者颜色输入错误！" << std::endl;
//ResCenters.push_back(cv::Point3i(0, 0, mode));
return ResCenters;
}
srand(time(NULL));
for (int i = 0; i++ < 777777;)
{
int x = rand() % (530 - 2 * radius)+ 400 + radius; //仅产生小桌板处的随机坐标
int y = rand() % (530 - 2 * radius)+ 50 + radius;
//std::cout << "随机数：（" << x << ", " << y << "）" << std::endl;
cv::Rect rect = cv::Rect(x - radius, y - radius, 2 * radius, 2 * radius);
if (cv::countNonZero(binaryImg(rect)) > radius * radius * 4 * 0.77)
{
ResCenters.push_back(cv::Point3i(x, y, mode));
break;
}
}
for (std::vector<cv::Point3i>::iterator it = ResCenters.begin(); it != ResCenters.end(); ++it)
{
cv::circle(im, cv::Point2i(it->x, it->y), 3, cv::Scalar(0, 255, 0), -1);
cv::circle(colormask, cv::Point2i(it->x, it->y), 3, cv::Scalar(0, 255, 0), -1);
std::cout << "类别：" << it->z << ", 坐标：(" << it->x << ", " << it->y << "）" << std::endl;
}
//cv::imshow("im", im);
//cv::imshow("img", colormask);
//cv::waitKey(0);
//ROS_INFO("111");
return ResCenters;
}
//Picture Process Class
class RGB_GRAY
{
private:
ros::NodeHandle nh_; //定义ROS句柄
image_transport::ImageTransport it_; //定义一个image_transport实例
image_transport::Subscriber image_sub_; //定义ROS-Color图象接收器
//image_transport::Publisher image_pub_; //定义ROS图象发布器
public:
RGB_GRAY()
: it_(nh_) //构造函数
{
image_sub_ = it_.subscribe("/camera/color/image_raw", 1, &RGB_GRAY::convert_callback, this); //定义图象接受器，订阅话题是“camera/rgb/image_raw”
// image_pub_ = it_.publishe("", 1); //定义图象发布器
//初始化输入输出窗口
// cv::namedWindow(INPUT);
// cv::namedWindow(OUTPUT);
}
~RGB_GRAY() //析构函数
{
// cv::destroyWindow(INPUT);
// cv::destroyWindow(OUTPUT);
}
/*
这是一个ROS和OpenCV的格式转换回调函数，将图象格式从sensor_msgs/Image ---> cv::Mat
*/
void convert_callback(const sensor_msgs::ImageConstPtr &msg)
{
if (CAMERA_CB_FLAG == false)
{
return;
}
else
{
cv_bridge::CvImagePtr cv_ptr; // 声明一个CvImage指针的实例
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); //将ROS消息中的图象信息提取，生成新cv类型的图象，复制给CvImage指针
}
//, sensor_msgs::image_encodings::RGB8
catch (cv_bridge::Exception &e) //异常处理
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
image_process(cv_ptr->image); //得到了cv::Mat类型的图象，在CvImage指针的image中，将结果传送给处理函数
}
}
/*
这是图象处理的主要函数，一般会把图像处理的主要程序写在这个函数中。这里的例子只是一个彩色图象到灰度图象的转化
*/
void image_process(cv::Mat img)
{
lunyi li;
std::vector<cv::Point3i> point = li.lunyi_jieti(img, 'R');
ROS_INFO("R finished");
std::vector<cv::Point3i> point2 = li.lunyi_jieti(img, 'Y');
ROS_INFO("Y finished");
std::vector<cv::Point3i> point3 = li.lunyi_jieti(img, 'O');
ROS_INFO("O finished");
// std::vector< std::vector<cv::Point3i>> result;
// Final_Centers.push_back(point);
// Final_Centers.push_back(point2);
// Final_Centers.push_back(point3);
point.insert(point.end(),point2.begin(),point2.end());
point.insert(point.end(),point3.begin(),point3.end());
Final_Centers = point;
}
};
//VOICD_ADD
bool flag_VOICE = false;
bool flag_PHONE = false;
string choice("");
string phone_command;
string y;
float pix, piy;
bool state = false;
void Callback(const std_msgs::String::ConstPtr &msg)
{
if (flag_VOICE == false)
{
return;
}
else
{
string s;
s = msg->data.c_str();
cout << s << endl;
choice = s;
}
}
//VOICD_ADD
void start_func()
{
ArmPart armpart;
ros::Rate rate(1);
// Get the fork
arm_pose_command[0] = 0.376922905445;
arm_pose_command[1] = -0.191357508302;
arm_pose_command[2] = 0.369895404577;
arm_pose_command[3] = 0.745306372643;
arm_pose_command[4] = 0.66345846653;
arm_pose_command[5] = -0.0259943697602;
arm_pose_command[6] = 0.0605439990759;
finger_pos_command[0] = 0;
finger_pos_command[1] = 0;
finger_pos_command[2] = 0;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
arm_pose_command[2] = 0.269895404577;
finger_pos_command[0] = 1;
finger_pos_command[1] = 1;
finger_pos_command[2] = 1;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
arm_pose_command[2] = 0.409895404577;
armpart.arm_move_only(arm_pose_command);
solve(A, B, X, CV_SVD);
// armpart.joint_pose_client(HOME_JOINT_COMMAND);
// Realsense2_Camera View
arm_pose_command[0] = 0.197176724672;
arm_pose_command[1] = -0.382402718067;
arm_pose_command[2] = 0.469826817513;
arm_pose_command[3] = 0.752546548843;
arm_pose_command[4] = 0.658056497574;
arm_pose_command[5] = 0.00997944921255;
arm_pose_command[6] = 0.0231453701854;
finger_pos_command[0] = 1;
finger_pos_command[1] = 1;
finger_pos_command[2] = 1;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
}
void stop_func()
{
ArmPart armpart;
ros::Rate rate(1);
// Calculate for the pixel to base Transform
solve(A, B, X, CV_SVD);
// Return the fork
arm_pose_command[0] = 0.379164546728;
arm_pose_command[1] = -0.172850057483;
arm_pose_command[2] = 0.438311493397;
arm_pose_command[3] = 0.77736312151;
arm_pose_command[4] = 0.617585599422;
arm_pose_command[5] = -0.0817226842046;
arm_pose_command[6] = 0.0872707143426;
armpart.arm_move_only(arm_pose_command);
arm_pose_command[2] = 0.338311493397;
finger_pos_command[0] = 0;
finger_pos_command[1] = 0;
finger_pos_command[2] = 0;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
arm_pose_command[2] = 0.438311493397;
armpart.arm_move_only(arm_pose_command);
arm_pose_command[0] = 0.277176724672;
arm_pose_command[1] = -0.0718067;
arm_pose_command[2] = 0.469826817513;
arm_pose_command[3] = 0.752546548843;
arm_pose_command[4] = 0.658056497574;
arm_pose_command[5] = 0.00997944921255;
arm_pose_command[6] = 0.0231453701854;
finger_pos_command[0] = 0;
finger_pos_command[1] = 0;
finger_pos_command[2] = 0;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
}
void catch_func(string str)
{
ArmPart armpart;
ros::Rate rate(1);
// Calculate for the pixel to base Transform
solve(A, B, X, CV_SVD);
// armpart.joint_pose_client(HOME_JOINT_COMMAND);
ros::Duration(1).sleep(); // sleep for half a second
CAMERA_CB_FLAG = true;
// Begin-----*********Vision Test Open bottle cup*********//
while (ros::ok())
{
ros::spinOnce();
rate.sleep();
if (CAMERA_CB_FLAG == true)
{
ROS_INFO("The Camera Callback");
// for (int i = 0; i < 9; i++)
// {
// for (int j = 0; j < 3; j++)
// {
// pixel[i][j] = results[i][j];
// }
// }
CAMERA_CB_FLAG = false;
break;
}
}
for (std::vector<cv::Point3i>::iterator it = Final_Centers.begin(); it != Final_Centers.end(); ++it)
{
std::cout<<it->z<<str[0]<<(it->z == str[0])<<std::endl;
if(it->z == str[0])
{
// pix = it->x+77;
// piy = it->y+36;
pix = it->x+70;
piy = it->y+60;
std::cout<<"Current Fruit Pixel"<<std::endl;
std::cout<<"Pix = " <<pix<<std::endl;
std::cout<<"Piy = " <<piy<<std::endl;
}
else
{
std::cout<<"No Fruit Detected" <<std::endl;
}
}
// if (str == "red")
// {
// pix = Final_Centers[0].x+77, piy = Final_Centers[0].y+36;
// pix = 571, piy = 223;
// }
// else if (str == "orange")
// {
// pix = Final_Centers[2].x+79, piy = Final_Centers[2].y+73;
// pix = 587, piy = 389;
// }
// else if (str == "white")
// {
// pix = Final_Centers[1].x+45, piy = Final_Centers[1].y+64;
// pix = 752, piy = 412;
//}
Mat a1 = (Mat_<float>(1, 3) << pix, piy, 1);
Mat b1 = a1 * X;
kx = b1.at<float>(0, 0);
ky = b1.at<float>(0, 1);
cout << "Kx = " << kx << endl;
cout << "Ky = " << ky << endl;
// arm_pose_command[0] = kx+0.015;
// arm_pose_command[1] = ky+0.015;
arm_pose_command[0] = kx;
arm_pose_command[1] = ky;
arm_pose_command[2] = 0.469826817513;
arm_pose_command[3] = 0.752546548843;
arm_pose_command[4] = 0.658056497574;
arm_pose_command[5] = 0.00997944921255;
arm_pose_command[6] = 0.0231453701854;
finger_pos_command[0] = 1;
finger_pos_command[1] = 1;
finger_pos_command[2] = 1;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
// finger_pos_command[0] = 0;
// finger_pos_command[1] = 0;
// finger_pos_command[2] = 0;
// // armpart.arm_and_gripper(arm_pose_command,finger_pos_command);
// armpart.finger_control_client(finger_pos_command);
arm_pose_command[2] = 0.3;
armpart.arm_move_only(arm_pose_command);
arm_pose_command[2] = 0.25769386482-0.008*(pix-140)/380;
armpart.arm_move_only(arm_pose_command);
arm_pose_command[2] = 0.4;
armpart.arm_move_only(arm_pose_command);
arm_pose_command[0] = 0.0124750956893;
arm_pose_command[1] = -0.256390303373;
arm_pose_command[2] = 0.553701460361;
arm_pose_command[3] = 0.750686883926;
arm_pose_command[4] = 0.0847822874784;
arm_pose_command[5] = -0.500494539738;
arm_pose_command[6] = 0.42283141613;
armpart.arm_move_only(arm_pose_command);
ros::Duration(10).sleep(); // sleep for half a second
arm_pose_command[0] = 0.197176724672;
arm_pose_command[1] = -0.382402718067;
arm_pose_command[2] = 0.469826817513;
armpart.arm_move_only(arm_pose_command);
arm_pose_command[3] = 0.752546548843;
arm_pose_command[4] = 0.658056497574;
arm_pose_command[5] = 0.00997944921255;
arm_pose_command[6] = 0.0231453701854;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
}
void arm_action()
{
if (phone_command == "start" && state == false)
{
ROS_INFO("The start Command");
start_func();
state = true;
}
else if (phone_command == "stop" && state == true)
{
ROS_INFO("The stop Command");
stop_func();
state = false;
}
else
{
if (state == true)
{
ROS_INFO("The catch fruit Command");
catch_func(phone_command);
}
else
{
std::cout << "Start First Please" << endl;
}
}
}
// Phone Callback
void Phone_Callback(const std_msgs::String::ConstPtr &msg)
{
if (flag_PHONE == false)
{
return;
}
else
{
string s;
s = msg->data.c_str();
cout <<"phone callback cmd: "<< s << endl;
phone_command = s;
arm_action();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle node_handle;
ros::Rate rate(1);
//VOICE_ADD
ros::Publisher voice_pub = node_handle.advertise<std_msgs::String>("xfwakeup", 1000); //wakeup
ros::Subscriber voice_sub = node_handle.subscribe("xfspeech", 1000, Callback); //receive msg
ros::Subscriber phone_sub = node_handle.subscribe("For_Kinova_arm", 1, Phone_Callback); //receive msg
//VOICD_ADD
RGB_GRAY obj;
ArmPart armpart;
// Calculate for the pixel to base Transform
solve(A, B, X, CV_SVD);
kinova_msgs::HomeArm srv;
armpart.registerNodeHandle(node_handle);
armpart.registerSrv();
// armpart.GotoHome(srv);
armpart.registerSub();
// armpart.joint_pose_client(HOME_JOINT_COMMAND);
ROS_INFO("Robot Start!");
flag_PHONE = true;
//ros::spin();
// CAMERA_CB_FLAG = true;
// Begin-----*********Vision Test Open bottle cup*********//
while (ros::ok())
{
ros::spinOnce();
rate.sleep();
if (flag_PHONE == true)
{
// for (int i = 0; i < 9; i++)
// {
// for (int j = 0; j < 3; j++)
// {
// pixel[i][j] = results[i][j];
// }
// }
// CAMERA_CB_FLAG = false;
// break;
}
}
std::cout << "Final_Centers[0].x: " << Final_Centers[0].x;
std::cout << "Final_Centers[0].y: " << Final_Centers[0].y;
std::cout << "Final_Centers[1].x: " << Final_Centers[1].x;
std::cout << "Final_Centers[1].y: " << Final_Centers[1].y;
std::cout << "Final_Centers[2].x: " << Final_Centers[2].x;
std::cout << "Final_Centers[2].y: " << Final_Centers[2].y;
return 0;
// Realsense2_Camera View
arm_pose_command[0] = 0.197176724672;
arm_pose_command[1] = -0.382402718067;
arm_pose_command[2] = 0.469826817513;
arm_pose_command[3] = 0.752546548843;
arm_pose_command[4] = 0.658056497574;
arm_pose_command[5] = 0.00997944921255;
arm_pose_command[6] = 0.0231453701854;
finger_pos_command[0] = 0;
finger_pos_command[1] = 0;
finger_pos_command[2] = 0;
armpart.arm_and_gripper(arm_pose_command, finger_pos_command);
ros::Duration(1).sleep(); // sleep for half a second
CAMERA_CB_FLAG = true;
// Begin-----*********Vision Test Open bottle cup*********//
while (ros::ok())
{
ros::spinOnce();
rate.sleep();
if (CAMERA_CB_FLAG == true)
{
ROS_INFO("The Camera Callback");
CAMERA_CB_FLAG = false;
break;
}
}
std::cout << "Final_Centers"<< std::endl;
std::cout << Final_Centers[0].x;
std::cout << Final_Centers[0].y;
return 0;
//VOICE_ADD
//loop:
// cout << "Please input y to continue:" << endl;
// cin >> y;
// if(y == "y"){
// std_msgs::String msg;
// std::stringstream ss;
// cout << "Ask for speak(from xfwakeup)." << endl;
// ss << "[xfwakeup_sub]";
// msg.data = ss.str();
// voice_pub.publish(msg);
// }else{
// return 0;
// }
// int cnt = 15;
// while(flag_VOICE && cnt !=0){
// ros::spinOnce();
// cnt--;
// sleep(1);
// }
// flag_VOICE = 1;
//VOICD_ADD
// if(choice == "watermalon")
// {
// pix=Final_Centers[0].x,piy=Final_Centers[0].y;
// } else if(choice == "hami")
// {
// pix=Final_Centers[1].x,piy=Final_Centers[1].y;
// } else if(choice =="casaba")
// {
// pix=Final_Centers[2].x,piy=Final_Centers[2].y;
// }else{
// cout << "No key words, please repeat again." << endl;
// goto loop;
// }
}
3317
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
