使用越疆科技的M1-B1机器人进行ROS下移动加机械臂加视觉抓取代码
使用越疆科技的M1-B1机器人进行ROS下移动加机械臂加视觉抓取代码
#include "ros/ros.h"
#include "ar_track_alvar_msgs/AlvarMarkers.h"
#include "iostream"
#include "stdio.h"
#include "cv.h"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"
#include
#include
#include
#include
#include
#include
#include "std_msgs/String.h"
#include "dobot/SetCmdTimeout.h"
#include "dobot/SetQueuedCmdClear.h"
#include "dobot/SetQueuedCmdStartExec.h"
#include "dobot/SetQueuedCmdForceStopExec.h"
#include "dobot/GetDeviceVersion.h"
#include "dobot/SetEndEffectorParams.h"
#include "dobot/SetPTPJointParams.h"
#include "dobot/SetPTPCoordinateParams.h"
#include "dobot/SetPTPJumpParams.h"
#include "dobot/SetPTPCommonParams.h"
#include "dobot/SetPTPCmd.h"
#include "dobot/SetEndEffectorGripper.h"
#include "dobot/SetHOMECmd.h"
#include "dobot/SetHOMEParams.h"
#include "dobot/SetIODO.h"
#include "dobot/GetAlarmsState.h"
#include "dobot/ClearAllAlarmsState.h"
using namespace std;
using namespace cv;
////////////////////////相机区域
//机械臂拍照点位
float picture_x = 237.633
, picture_y = 60.6892
, picture_z = 230.7879
;
//标定像素坐标 3个点
float cam_x1 = 159.387
, cam_y1 = -21.6633
, cam_x2 = 34.927
, cam_y2 = 40.634
, cam_x3 = 165.001
, cam_y3 = 81.35
;
//标定机械包坐标 3个点
float rob_x1 = 327.7778
, rob_y1 = -78.2467
, rob_x2 = 357.0341
, rob_y2 = 43.5529
, rob_x3 = 250.8334
, rob_y3 = -21.1507
;
//标定像素坐标 3个点
Mat A = (Mat_(3,3) <<
cam_x1, cam_y1, 1,
cam_x2, cam_y2, 1,
cam_x3, cam_y3, 1
); //3×3
//标定机械包坐标 3个点
Mat B = (Mat_(3,3) <<
rob_x1, rob_y1, 1,
rob_x2, rob_y2, 1,
rob_x3, rob_y3, 1
); //3×3
//矩阵参数
Mat X;
//相机得到图像的坐标和ID
int ar_ID;
float cam_getX;
float cam_getY;
//机械臂最后执行的点位
float X_pose;
float Y_pose;
float Z_pose = 155.9403;
//服务注册
ros::ServiceClient client;
ros::ServiceClient clientPTP;
ros::ServiceClient clientIO;
//函数声明
void robot_pose(Mat X);
void setIO(uint8_t address, uint8_t level, bool isQueued, ros::ServiceClient client);
void setPTP(uint8_t ptpMode ,float x, float y, float z, float r, ros::ServiceClient client);
int getState(ros::ServiceClient client);
int ClearState(ros::ServiceClient client);
void sub_ar_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg);
//回调函数获取二维码的位置
void sub_ar_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
ros::ServiceClient client;
cout<<"回调函数获取二维码的位置" <markers.size() > 0) && (msg->markers[0].id < 30) )
if(msg->markers.size() > 0 && (msg->markers[0].id < 30) )
{
ar_ID = msg->markers[0].id;
cam_getX = msg->markers[0].pose.pose.position.x*1000;
cam_getY = msg->markers[0].pose.pose.position.y*1000;
cout<<"获取二维码的位置"<(1,3) << cam_getX, cam_getY, 1);
//通过矩阵变换得到机器人抓取的物理坐标
Mat rot_pose = cam_pose*X;
//机械臂抓取点位输出
X_pose = rot_pose.at(0);
Y_pose = rot_pose.at(1);
cout<<"抓取点位："<= 0 && ros::ok())
{
ros::spinOnce();
i = i-1;
cout<<"call :"< ac("move_base",true);
//设置我们要机器人走的几个点。
geometry_msgs::Point point;
geometry_msgs::Quaternion quaternion;
geometry_msgs::Pose pose_list1;
geometry_msgs::Pose pose_list2;
point.x = -0.174;
point.y = -0.921;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = -0.728;
quaternion.w = 0.684;
pose_list1.position = point;
pose_list1.orientation = quaternion;
point.x = 1.842;
point.y = 0.893;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = -0.0331;
quaternion.w = 0.999;
pose_list2.position = point;
pose_list2.orientation = quaternion;
// SetCmdTimeout
client = n.serviceClient("/DobotServer/SetCmdTimeout");
dobot::SetCmdTimeout srv1;
srv1.request.timeout = 3000;
if (client.call(srv1) == false) {
ROS_ERROR("Failed to call SetCmdTimeout. Maybe DobotServer isn't started yet!");
return -1;
}
// Clear the command queue
client = n.serviceClient("/DobotServer/SetQueuedCmdClear");
dobot::SetQueuedCmdClear srv2;
client.call(srv2);
// Start running the command queue
client = n.serviceClient("/DobotServer/SetQueuedCmdStartExec");
dobot::SetQueuedCmdStartExec srv3;
client.call(srv3);
// Get device version information
client = n.serviceClient("/DobotServer/GetDeviceVersion");
dobot::GetDeviceVersion srv4;
client.call(srv4);
if (srv4.response.result == 0) {
ROS_INFO("Device version:%d.%d.%d", srv4.response.majorVersion, srv4.response.minorVersion, srv4.response.revision);
} else {
ROS_ERROR("Failed to get device version information!");
}
// Set PTP joint parameters
do {
client = n.serviceClient("/DobotServer/SetPTPJointParams");
dobot::SetPTPJointParams srv;
for (int i = 0; i < 4; i++) {
srv.request.velocity.push_back(100);
}
for (int i = 0; i < 4; i++) {
srv.request.acceleration.push_back(100);
}
client.call(srv);
} while (0);
// Set PTP coordinate parameters
do {
client = n.serviceClient("/DobotServer/SetPTPCoordinateParams");
dobot::SetPTPCoordinateParams srv;
srv.request.xyzVelocity = 100;
srv.request.xyzAcceleration = 100;
srv.request.rVelocity = 100;
srv.request.rAcceleration = 100;
client.call(srv);
} while (0);
// Set PTP jump parameters
do {
client = n.serviceClient("/DobotServer/SetPTPJumpParams");
dobot::SetPTPJumpParams srv;
srv.request.jumpHeight = 20;
srv.request.zLimit = 200;
client.call(srv);
} while (0);
// Set PTP common parameters
do {
client = n.serviceClient("/DobotServer/SetPTPCommonParams");
dobot::SetPTPCommonParams srv;
srv.request.velocityRatio = 50;
srv.request.accelerationRatio = 50;
client.call(srv);
} while (0);
//运行到拍照点位PTP
client = n.serviceClient("/DobotServer/SetPTPCmd");
setPTP(0 ,picture_x ,picture_y ,picture_z ,0 , client);
//运行到抓取点位PTP
clientPTP = n.serviceClient("/DobotServer/SetPTPCmd");
//开启气泵setIO
clientIO = n.serviceClient("/DobotServer/SetIODO");
cout<<"机械臂准备完成请按下空格键开始检测"<