ros开发增加clion常用模板及初始化配置(三)
ros开发增加clion常用模板及初始化配置(三)
python模板
py_math弧度转角度
import math
DE2R = math.pi/180
#弧度=角度*DE2Rpy_unpack通信大端字节转换_小端
def get_header(self, data):
return struct.unpack("!iB", data[0:5])
psize, ptype = self.get_header(data)py_socket通讯收发数据
import socket
import math
DE2R = math.pi/180
# 创建socket连接
conn = socket.create_connection(('192.168.36.28',30002))
# 发送指令
msg = 'movej([{},{},{},{},{},{}], a=1.4, v=1.05, t=0, r=0)\n'.format(-144.98 * DE2R, -97.67 * DE2R, -102.98 * DE2R,-68.95 * DE2R, 83.07 * DE2R, 58.15 * DE2R)
# conn.send(msg.encode())
movelMsg = 'movel(p[{},{},{},{},{},{}], a=1.2, v=0.25, t=0, r=0)\n'.format(-54.16/1000,-324.52/1000,183.76/1000,3.1225,0.5556,0.2693)
conn.send(movelMsg.encode())py_filter过滤器
pressPoints = list(filter(lambda ele:ele['type']==Point.PRESS,self.paintWidget.point.data))py_qt_btn按钮点击事件
# 绘制点击事件
paintBtn.clicked.connect(self.paint)py_opencv_waitKey_VideoCapture按空格保存图片
video = cv.VideoCapture(0)
# 判断视频是否打开成功
isOpened = video.isOpened()
print("视频是否打开成功：",isOpened)
# 获取图片的信息:帧率
fps = video.get(cv.CAP_PROP_FPS)
# 获取每帧宽度
width = video.get(cv.CAP_PROP_FRAME_WIDTH)
# 获取每帧的高度
height = video.get(cv.CAP_PROP_FRAME_HEIGHT)
print("帧率：{},宽度：{},高度：{}".format(fps,width,height))
# 从视频中读取8帧信息
count=0
while True:
count = count + 1
# 读取成功or失败， 当前帧数据
flag,frame = video.read()
#frame=cv.flip(frame, 1) #镜像
cv.imshow("frame1", frame);#显示视频
action =cv.waitKey(10) & 0xFF
ACTION_ESC = 27;
ACTION_SPACE = 32;
if (action == ACTION_SPACE or action == 'q'):
aaa(frame )
return -1
if (action == ACTION_ESC ):
return -1
# 将图片信息写入到文件中
if flag: # 保存
# 图片的质量
#cv.imwrite("img/tiger%d.jpg"%count,frame,[cv.IMWRITE_JPEG_QUALITY,100])
pass
print("图片截取完成啦！")
#cv.waitKey(0)
cv.destroyAllWindows()
py_opencv_HSV_Hsv_change调试类
import cv2 as cv
class Hsv_change:
#hsv 调试
def __init__(self):
self.i1 = 0
self.i2 = 0
self.i3 = 0
self.i4 = 0
self.i5 = 0
self.i6 = 0
pass
def onChange(self,a, b, hsv):
if a == 1:
self.i1 = b
elif a == 2:
self.i2 = b
elif a == 3:
self.i3 = b
elif a == 4:
self.i4 = b
elif a == 5:
self.i5 = b
elif a == 6:
self.i6 = b
lowerb = (self.i1, self.i2, self.i3)
upperb = (self.i4, self.i5, self.i6)
mask = cv.inRange(hsv, lowerb, upperb)
cv.imshow("mask", mask)
print(self.i1, self.i2, self.i3, self.i4, self.i5, self.i6)
## 调用次方法,传入文件路径
def inRange_byFilePath(self,pathFile="img/tiger261.jpg"):
# img = cv2.imread("data2/test_aubo2.png")
# img_copy=img.copy()
dstImgggg = cv.imread(pathFile)
img_copysize = cv.resize(dstImgggg, (int(dstImgggg.shape[1] / 2), int(dstImgggg.shape[0] / 2)))
cv.imshow("img_copysize", img_copysize)
hsv = cv.cvtColor(img_copysize, cv.COLOR_BGR2HSV)
cv.imshow("hsv", hsv)
cv.namedWindow("mask")
cv.createTrackbar("h1", "mask", 0, 255, lambda a: self.onChange(1, a, hsv))
cv.createTrackbar("s1", "mask", 0, 255, lambda a:self.onChange(2, a, hsv))
cv.createTrackbar("v1", "mask", 0, 255, lambda a:self.onChange(3, a, hsv))
cv.createTrackbar("h2", "mask", 0, 255, lambda a:self.onChange(4, a, hsv))
cv.createTrackbar("s2", "mask", 0, 255, lambda a:self.onChange(5, a, hsv))
cv.createTrackbar("v2", "mask", 0, 255, lambda a:self.onChange(6, a, hsv), )
cv.waitKey(0)
cv.destroyAllWindows()
## 调用次方法,传入图片
def inRange_byImage(self, dstImgggg):
img_copysize = cv.resize(dstImgggg, (int(dstImgggg.shape[1] / 2), int(dstImgggg.shape[0] / 2)))
cv.imshow("img_copysize", img_copysize)
hsv = cv.cvtColor(img_copysize, cv.COLOR_BGR2HSV)
cv.imshow("hsv", hsv)
cv.namedWindow("mask")
cv.createTrackbar("h1", "mask", 0, 255, lambda a: self.onChange(1, a, hsv))
cv.createTrackbar("s1", "mask", 0, 255, lambda a: self.onChange(2, a, hsv))
cv.createTrackbar("v1", "mask", 0, 255, lambda a: self.onChange(3, a, hsv))
cv.createTrackbar("h2", "mask", 0, 255, lambda a: self.onChange(4, a, hsv))
cv.createTrackbar("s2", "mask", 0, 255, lambda a: self.onChange(5, a, hsv))
cv.createTrackbar("v2", "mask", 0, 255, lambda a: self.onChange(6, a, hsv), )
cv.waitKey(0)
cv.destroyAllWindows()
if __name__ == '__main__':
##使用:
h=Hsv_change()
h.inRange_byFilePath("img/tiger261.jpg")py_os_popen获取绝对路径
#绝对路径
print("{}/img/output.svg".format(os.popen("pwd").readline().rstrip('\r\n')))py_三目运算
print(x if (x > y) else y)
print((x if (x>y) else y) if ((x if (x>y) else y)>z) else z)py_str字符串查找截取替换
# 在字符串str里查找字符串hello
str.find('hello')
# 将字符串里的k全部替换为8
str.replace('k',' 8')
#指定分隔符为'.'
str.split('.')
#指定分割次数
str.split('.',2)
##字符串截取,如包含点的就截取,不包含点的就不截取;
h_index=h.find(".")
y = h[:h_index if (h_index>1) else len(h)]py_opencv原始画布与现有画布图像坐标转换
def scaleData_with_wh_svg(self,px,py,w,h):
'''
对数据缩放处理
:param cal:
:return:
px :原图像x坐标
py :原图像y坐标
w :原图像所在的画布宽度
h :原图像所在的画布宽度
映射的画布尺寸 w:640 h: 480
1.取最小的(高度最小) h: y_p高度尺寸,按原始画布的宽高比例计算出宽度尺寸x_p
2.通过原始图像px坐标与原始画布w宽度的比例计算出映射画布的图像x坐标
3.通过原始图像py坐标与原始画布h宽度的比例计算出映射画布的图像y坐标
'''
h_index=h.find(".")
w_index = w.find(".")
h=int(h[:h_index if (h_index>1) else len(h)])
w=int(w[:w_index if (w_index>1) else len(w)])
#1.取最小的(高度最小) h: y_p高度尺寸, 按原始画布的宽高比例计算出宽度尺寸x_p
y_p = 480
x_p = y_p * h / w
#2.通过原始图像px坐标与原始画布w宽度的比例计算出映射画布的图像x坐标
x = px *x_p / h
#3.通过原始图像py坐标与原始画布h宽度的比例计算出映射画布的图像y坐标
y = py * y_p/ w
return x,ypy_ros_机器人写字_svg图片绘制_二三阶贝塞尔
# ------------------------- 绘制二阶贝塞尔 -------------------------#
def QuadraticBezier(self,ps, pc, pe, t):
'''
获取二阶贝塞尔点
:param ps: 开始点
:param pc: 控制点
:param pe: 结束点
:param t: 0-1
:return:
'''
return pow(1 - t, 2) * ps + 2 * t * (1 - t) * pc + pow(t, 2) * pe
def drawQuadraticBezier(self,ele):
# 开始点
ps = np.array([ele.start.real, ele.start.imag])
# 控制点
p = np.array([ele.control.real, ele.control.imag])
# 结束点
pe = np.array([ele.end.real, ele.end.imag])
point = self.QuadraticBezier(ps, p, pe, 0)
start = int(point[0]), int(point[1])
z = 30.25
# 创建点
point = {'x': start[0], 'y': start[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
# 添加到容器中
self.points.append(point)
# 40个点
for i in range(1, 41):
result = self.QuadraticBezier(ps, p, pe, i / 40)
end = int(result[0]), int(result[1])
# 创建点
point = {'x': end[0], 'y': end[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
# 添加到容器中
self.points.append(point)
# 连接两个点
# cv.line(self.dst, start, end, self.randomColor())
# cv.imshow('dst', self.dst)
# cv.waitKey(5)
# 开始点变成结束点
#start = end
# ------------------------- 三阶贝塞尔 -------------------------#
def CubicBezier(self,ps, pc1, pc2, pe, t):
'''
获取二阶贝塞尔点
:param ps: 开始点
:param pc: 控制点
:param pe: 结束点
:param t: 0-1
:return:
'''
return pow(1 - t, 3) * ps + 3 * t * pow(1 - t, 2) * pc1 + 3 * pow(t, 2) * (1 - t) * pc2 + pow(t, 3) * pe
def drawCubicBezier(self,ele):
print('绘制贝塞尔')
# 开始点
ps = np.array([ele.start.real, ele.start.imag])
# 控制点
p1 = np.array([ele.control1.real, ele.control1.imag])
p2 = np.array([ele.control2.real, ele.control2.imag])
# 结束点
pe = np.array([ele.end.real, ele.end.imag])
result = self.CubicBezier(ps, p1, p2, pe, 0)
print(result)
start = int(result[0]), int(result[1])
z = 30.25
# 创建点
point = {'x': start[0], 'y': start[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
# 添加到容器中
self.points.append(point)
# 40个点
for i in range(1, 41):
result = self.CubicBezier(ps, p1, p2, pe, i / 40)
end = int(result[0]), int(result[1])
# 创建点
point = {'x': end[0], 'y': end[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
# 添加到容器中
self.points.append(point)
# 连接两个点
# cv.line(self.dst, start, end, self.randomColor())
# cv.imshow('dst', self.dst)
# cv.waitKey(5)
# # 开始点变成结束点
# start = endpy_qt_ QTimer定时器
from PyQt5.QtCore import QTimer,pyqtSignal,Qt
# 自定义 timer， 负责页面更新
update_timer = QTimer(self)
update_timer.setInterval(16)
update_timer.start()
# timer事件回调
update_timer.timeout.connect(self.on_update)
def on_update(self):
# qt刷新
self.update()
# 判断用户终止操作
if rospy.is_shutdown():
self.close()py_os_获取当前路径
import os
## /home/sk/workspas/demo_ur/src/demour/scripts1
print("{}".format(os.popen("pwd").readline().rstrip('\r\n')))
==========================================================================================
------------------------------------------------------------------------------------------------------------------------------------------------
c++模板
cpp_pcl_NormalEstimation积分图估算点云法线_main ------------------
//
// Created by sk on 2020/8/5.
//
#include
#include
#include
#include
#include
#include
using namespace std;
/**
* 积分图估算点云法线
此方法进行法线估计只适用于有序点云,对于无序点云就只能采用其他方法。
*/
int main(int argc, char *argv[]) {
char *arg = argv[1];
if (!argv[1]) arg = "./data/table_scene_mug_stereo_textured.pcd";
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
// pcl::PointCloud::Ptr cloud_downsampled(new pcl::PointCloud);
pcl::io::loadPCDFile(arg, *cloud);
// 创建法线集合
pcl::PointCloud::Ptr normals(new pcl::PointCloud);
// 创建一个积分图法线评估对象
pcl::IntegralImageNormalEstimation iine;
// 设置法线估算方式
/**
enum NormalEstimationMethod
COVARIANCE_MATRIX : 模式创建9个积分图像，以根据其局部邻域的协方差矩阵为>特定点计算法线
AVERAGE_3D_GRADIENT: 模式创建6个积分图像以计算水平和垂直3D渐变的平滑版本
，并使用这两个渐变之间的叉积计算法线。
AVERAGE_DEPTH_CHANGE: 模式仅创建单个积分图像，并根据平均深度变化计算法线
。
*/
iine.setNormalEstimationMethod(iine.AVERAGE_3D_GRADIENT);
// 设置最大深度变换因子
iine.setMaxDepthChangeFactor(0.02f);
iine.setNormalSmoothingSize(10.0f);
iine.setInputCloud(cloud);
iine.compute(*normals);
pcl::visualization::PCLVisualizer viewer("PCLViewer");
viewer.addPointCloud(cloud, "cloud");
// 每几个点绘制一个法向量
int level = 10;
float scale = 0.02;
viewer.addPointCloudNormals(cloud, normals, level, scale, "normal_cloud");
viewer.addCoordinateSystem(0.01);
while (!viewer.wasStopped()) {
viewer.spinOnce();
std::this_thread::sleep_for(100ms);
}
}
cpp_function包装函数_回调函数
#include
function connectCallBack; cpp_shared_ptr
shared_ptr URDriver::instance = shared_ptr(new URDriver); cpp_ros_ur_movej_movel指令发送
//movej
QString URScript::loadMovejScript(double *joints, double a, double v) {
//'movej([90,80], a=1.4, v=1.05, t=0, r=0)'
QString msg{"movej(["};
for (int i = 0; i < 6; ++i) {
msg += QString::number(joints[i]);
if (i != 5) {
msg += ",";
}
}
msg += "], a=";
msg += QString::number(a);
msg += ", v=";
msg += QString::number(v);
//最后面 必要要有换行符
msg+=", t=0, r=0)\n";
return msg;
}
//movel
QString URScript::loadMovelScript(double *pose, double a, double v) {
//movel(pose, a=1.2, v=0.25, t=0, r=0)
QString msg{"movel(p["};
for (int i = 0; i < 6; ++i) {
msg += QString::number(pose[i]);
if (i != 5) {
msg += ",";
}
}
msg += "], a=";
msg += QString::number(a);
msg += ", v=";
msg += QString::number(v);
//最后面 必要要有换行符
msg+=", t=0, r=0)\n";
return msg;
}
//movej指令发送
void URDriver::movej(double joints[6],double a, double v) {
//发送指令给机械臂 'movej([90,80], a=1.4, v=1.05, t=0, r=0)'
//1.拼接发送的指令字符串
QString msg = script.loadMovejScript(joints,a,v);
//2.通过socket发送拼接的字符串
socket.write(msg.toUtf8());
}
//movel指令发送
void URDriver::movel(double *pose, double a, double v) {
//1.拼接发送的指令字符串
QString msg = script.loadMovelScript(pose,a,v);
//2.通过socket发送拼接的字符串
socket.write(msg.toUtf8());
}
//movej指令发送
void MainWindow::movej() {
//获取6个关节角度
double joint1 = joint1Edit->text().toDouble()*DEGREETORADIUS;
double joint2 = joint2Edit->text().toDouble()*DEGREETORADIUS;
double joint3 = joint3Edit->text().toDouble()*DEGREETORADIUS;
double joint4 = joint4Edit->text().toDouble()*DEGREETORADIUS;
double joint5 = joint5Edit->text().toDouble()*DEGREETORADIUS;
double joint6 = joint6Edit->text().toDouble()*DEGREETORADIUS;
//关节角度
double joints[]{joint1,joint2,joint3,joint4,joint5,joint6};
//移动机械臂
URDriver::getInstance()->movej(joints);
}
//movel指令发送
void MainWindow::movel() {
//获取位置和姿态
double x = xEdit->text().toDouble()/1000;
double y = yEdit->text().toDouble()/1000;
double z = zEdit->text().toDouble()/1000;
//传过来的已经是弧度了,如过来的是角度的话需要转弧度 角度*DEGREETORADIUS
double rx = rxEdit->text().toDouble();
double ry = ryEdit->text().toDouble();
double rz = rzEdit->text().toDouble();
//位置和姿态
double pose[]{x,y,z,rx,ry,rz};
//调用驱动的movel方法
URDriver::getInstance()->movel(pose);
}
cpp_opencv_waitKey按ESC停止_保存图片
int action = waitKey(30) & 0xFF;
if (action == ACTION_ESC) {
// 按下Esc停止采集，退出标定
std::cout << "用户主动停止标定" << std::endl;
return -1;
} else if (action == 'q' || action == 'Q') {
// 按下q或Q中断采集，开始执行标定
std::cout << "停止图片采集，开始标定" << std::endl;
break;
} else if (action == ACTION_SPACE) {
std::cout << "空格" << std::endl;
}cpp_opencv_CommandLineParser从外部获取参数
int main(int argc, char **argv) {
//获取外面键盘输入的参数
cv::CommandLineParser parser(argc, argv, "{@arg1||}{@arg2||}{@arg3||}{@arg4|15|}"
"{scale s|1.0|}{flip f||}{help h||}");//导入opencv2
if (parser.has("h")) {//从键盘获取 判断用户是否输入的h值
printHelp();
return 0;
}
int board_width = parser.get(0);//从键盘获取用户输入的第一位数字
int board_height = parser.get(1);//从键盘获取用户输入的第2位数字
int square_size = parser.get(2);//从键盘获取用户输入的第3位数字
int board_num = parser.get(3); //从键盘获取用户输入的第3位数字；ps：最大图片采集数量
float image_scale = parser.get("scale");//从键盘获取用户输入的scale 的值
bool flipHorizontal = parser.has("f");//从键盘获取用户输入的f或flip 的值
if (board_width < 1 || board_height < 1 || square_size <= 0) {
printHelp();
return -1;
}
std::cout << "board_width: " << board_width << std::endl;
std::cout << "board_height: " << board_height << std::endl;
std::cout << "square_size: " << square_size << std::endl;
std::cout << "board_num: " << board_num << std::endl;
std::cout << "image_scale: " << image_scale << std::endl;
std::cout << "flipHorizontal: " << flipHorizontal << std::endl; cpp_opencv_flip_resize图片镜像水平反转缩放
// 缩放
if (image_scale != 1.0) {
cv::resize(image0, image0, Size(), image_scale, image_scale);
}
// 水平反转
if (flipHorizontal) {
cv::flip(image0, image0, 1);
}cpp_opencv_FileStorage保存xml数据_读取xml数据
//------------------------保存数据---------------------------------
Mat cameraMatrix; // 相机内参
Mat distCoeffs; // 畸变系数
Mat rvecs, tvecs; // 标定板旋转平移姿态
// * 4. 执行标定 -> 相机内参+畸变系数
double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);
std::cout << "cameraMatrix: \n" << cameraMatrix << std::endl;
std::cout << "distCoeffs: \n" << distCoeffs << std::endl;
std::cout << "rms: \n" << rms << std::endl;
// * 5. 保存这些内参
time_t tm;
time(&tm);
struct tm *t2 = localtime(&tm);
char buf[1024];
strftime(buf, sizeof(buf), "%c", t2);
// 图片宽高，标定版数量，标定版尺寸，格子大小，时间
// FileStorage fs("./calibration.xml", FileStorage::WRITE);
FileStorage fs("./calibration.yml", FileStorage::WRITE);
fs << "time" << buf;
fs << "rms" << rms;
fs << "board_count" << (int)filePaths.size();
fs << "board_size" << patternSize; // 标定版尺寸
fs << "board_square" << square_size; // 格子大小
fs << "image_width" << imageSize.width;
fs << "image_height" << imageSize.height;
fs << "cameraMatrix" << cameraMatrix;
fs << "distCoeffs" << distCoeffs;
fs.release();
//------------------------读取数据---------------------------------
// 加载内参
FileStorage fs("./calibration.yml", FileStorage::READ);
int image_width{0}, image_height{0};
fs["image_width"] >> image_width;
fs["image_height"] >> image_height;
Mat cameraMatrix, distCoeffs;
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;
fs.release();
std::cout << "image_width: " << image_width << std::endl;
std::cout << "image_height: " << image_height << std::endl;
std::cout << "cameraMatrix: \n" << cameraMatrix << std::endl;
std::cout << "distCoeffs: \n" << distCoeffs << std::endl;
cpp_opencv_glob_filePaths加载文件夹下所有文件_image_*.jpg
vector filePaths;
cv::glob("./data/image_*.jpg", filePaths);
for (String filePath: filePaths) {
// 加载要去畸变图像
Mat src = imread(filePath, IMREAD_COLOR);
Mat dst;
// 对图像进行去畸变并展示
undistort(src, dst, cameraMatrix, distCoeffs);
imshow("src", src);
imshow("dst", dst);
waitKey(0);
} cpp_opencv_undistort对图像去畸变
//------------------------方法一-------------------------------
// 加载要去畸变图像
Mat src = imread(filePath, IMREAD_COLOR);
Mat dst;
// 对图像进行去畸变并展示 src去畸变图片,dst去完畸变的图片,cameraMatrix相机内参,distCoeffs畸变系数
undistort(src, dst, cameraMatrix, distCoeffs);
//------------------------二-------------------------------
// 初始化去畸变纠正变换Map
Mat map1, map2;
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
cameraMatrix, image_size, CV_16SC2, map1, map2);
Mat dst;
// 对图像进行去畸变并展示
remap(image, dst, map1, map2,
cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
imshow("image", dst);cpp_qt_socket_QTcpSocket发送_接收数据
#include
socket.connectToHost(ip, port);
//发送指令
socket.write(msg.toUtf8());
//监听connected信号
QObject::connect(&socket, &QTcpSocket::connected, [this] {
connectCallBack();
});
//断开连接信号
QObject::connect(&socket, &QTcpSocket::disconnected, [this] {
disConnectCallBack();
});
//读取返回的数据
QObject::connect(&socket, &QTcpSocket::readyRead, [this] {
//读取数据
QByteArray data = socket.readAll();
URData urData;
//解析数据
parseData(data, urData);
//决定是否执行下一条指令
decideExcuteNextInstruction(urData);
});