nao机器人避障边缘检测代码

最新推荐文章于 2023-04-26 07:26:21 发布
最新推荐文章于 2023-04-26 07:26:21 发布
阅读量5.4k 收藏 25
点赞数 3
分类专栏： NAO 机器人
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zouroot/article/details/50617657
版权

头文件.h

//common.h
#ifndef COMMON_MY_H
#define COMMON_MY_H

#include <iostream>
#include <alerror/alerror.h>
#include <alproxies/almotionproxy.h>
#include <vector>
#include <string>
using namespace std;

#include <alproxies/alvideodeviceproxy.h>
#include <alvision/alimage.h>
#include <alvision/alvisiondefinitions.h>
#include <alerror/alerror.h>
using namespace AL;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;

#include <Windows.h>
#include <time.h>

#define M_E	 2.718281828459
#define M_PI 3.1415926
#define M_2PI (3.1415926*2)
#define ZERO_DOUBLE 0.000000001
#define ZERO_CONTINE 0.1

void testBlue();
double ComputeTheta();//计算边线斜率角;
void testMouseAndHandle();
void testMove();
void testAction();
Mat& ScanImageAndReduceIterator(Mat& I, const uchar* const table);
Mat& TestNotGreen(Mat& I);
Mat& TestWhite(Mat& I);
int computePixel(double theta);
void testPixel(Mat &img,int col,int row);
void on_mouse( int event, int x, int y, int flags, void* ustc);
void testAction();
void Mat2Array(cv::Mat& img);
void bz();

struct Dis_flag
{
	double distance;
	int flag;//1表示白色，0表示蓝色,-1表示绿色
};

extern cv::Mat img;
extern double obstacles[320];//从60度开始扫描
extern Dis_flag obstacles2[320];


struct FreeDirectionNode
{
	int start;
	int end;
};

typedef FreeDirectionNode CollisionDirectionNode;

#else


#endif

功能函数实现 

//bz.cpp

#include "common.h"

void bz()
{
	//FILE* fp = fopen("test.txt","a+");\\192.168.1.81
	AL::ALMotionProxy motion("192.168.1.81", 9559);
	const std::string names = "Body";
	float stiffnessLists = 1.0f;
	float timeLists      = 1.0f;
	motion.stiffnessInterpolation(names, stiffnessLists, timeLists);

	ALVideoDeviceProxy camProxy("192.168.1.81", 9559);
	string clientName;
	//camProxy.unsubscribeAllInstances("test");
	clientName = camProxy.subscribe("test",kQVGA, kBGRColorSpace, 30);
	cout<<clientName.data()<<endl;
	camProxy.setActiveCamera(1);
	cv::Mat imgHeader = cv::Mat(cv::Size(320, 240), CV_8UC3);
	qi::os::msleep(1000);//等摄像头发布数据
	int step = 0;
	char buf_step[256];
	memset(buf_step,0,256);
	sprintf(buf_step,"%d.jpg",step);

	//终点的相对方向
	int n = 320;//感知精度
	int i_d=n/2;//目标在正前方
	double k = 100/240.0;
	double theta = atan(0.3/0.48)*360/6.28;
	double shang = k*sin(theta/360*6.28)+(1-k)*sin((47.64+theta)/360*6.28);//计算距离公式的上面
	double xia = k*cos(theta/360*6.28)+(1-k)*cos((47.64+theta)/360*6.28);//计算距离公式的下面
	double sensor_range=0.48*shang/xia*pow(M_E,fabs(k-1.0));//最大感知距离，扫描像素值100对应的距离
	double sensor_angle = 60.97/360*6.28;//感知角度范围
	double collision_distance = 0.5;
	double desiredDirection = 0;//方向在（M_PI/3,M_2PI/3）间
	clientName = camProxy.subscribe("test",kQVGA, kBGRColorSpace, 30);
	camProxy.setActiveCamera(1);
	int i = 0;
	while(waitKey(1) != 27)
	{
		//1.得到障碍物和边线信息
		//1.1获取图像
		ALValue img = camProxy.getImageRemote(clientName);
		imgHeader.data = (uchar*) img[6].GetBinary();
		//1.2显示图像
		//imgHeader = imread("images\\24.jpg");
		imshow("images", imgHeader);
		//1.3 保存图像
		//memset(buf_step,0,256);
		//sprintf(buf_step,"rx_images1\\%d.jpg",step++);
		//imwrite(buf_step, imgHeader);
		camProxy.releaseImage(clientName);
		//1.4转换为最近距离
		Mat2Array(imgHeader);//输出为obstacles数组
		//2.局部路径规划
//		imshow("changed",imgHeader);

		static bool mode = true;//直行模式
		int best_angle;

		//检测碰撞
		vector<int> freeDirectionPoints(n);
		int num_freeDirectionPoints = 0;//自由方向的数目
		for(int i = 0; i < n ;i++)
		{
			if(obstacles2[i].distance - collision_distance > ZERO_DOUBLE )
			{
				num_freeDirectionPoints++;
				freeDirectionPoints[i] = 0;//标记为自由方向
			}
			else
			{
				freeDirectionPoints[i] = 1;
			}
		}

		if(mode && num_freeDirectionPoints == n)//直行模式且未发生碰撞
		{
			//生成局部切线图(LTG)
			double m_F = obstacles2[i_d%n].distance;//目标方向上的自由距离
			double m_d = 5.0f;//当前机器人与目标的距离
			vector<int> idx;
			bool isInsert = false;
			for(int i = 0; i < n ;isInsert = false,i++)
			{
				if(fabs(obstacles2[i].distance - sensor_range) < ZERO_DOUBLE &&//剔除探测边界点
					fabs(obstacles2[(i+1)%n].distance - sensor_range) < ZERO_DOUBLE && 
					fabs(obstacles2[(i-1+n)%n].distance -sensor_range) < ZERO_DOUBLE )
				{
					isInsert = true;
				}
				if(!isInsert && fabs(obstacles2[i].distance - sensor_range) > ZERO_DOUBLE && 
					((fabs(obstacles2[(i+1)%n].distance - obstacles2[i].distance) > 0.2 && 
					fabs(obstacles2[(i-1+n)%n].distance - obstacles2[i].distance) < 0.2) || 
					(fabs(obstacles2[(i+1)%n].distance-obstacles2[i].distance) < 0.2 &&
					fabs(obstacles2[(i-1+n)%n].distance-obstacles2[i].distance) > 0.2)))//检测不连续
				{
					idx.push_back(i);
					isInsert = true;
				}
				if(!isInsert && (fabs(obstacles2[i].distance - sensor_range) < ZERO_DOUBLE))//检测与障碍物的交点
				{
					bool flag_min = true;//标记是否逐渐变小
					for(int j = 0; j < 10 && flag_min; j++)//假设连续10个变小就表示逐渐变小
					{
						if(obstacles2[(i+1+j)%n].distance > obstacles2[(i+j)%n].distance)
							flag_min = false;
					}
					if(flag_min)
					{
						idx.push_back(i);
						isInsert = true;
					}
				}
				if(!isInsert && (fabs(obstacles2[i%n].distance - sensor_range) < ZERO_DOUBLE))
				{
					bool flag_max = true;//标记是否逐渐变大
					for(int j = -9; j < 1 && flag_max; j++)//假设前面连续10个逐渐变大
					{
						if(obstacles2[(i+j+n)%n].distance > obstacles2[(i+j-1+n)%n].distance)
							flag_max = false;
					}
					if(flag_max)
					{
						idx.push_back(i);
					}
				}
			}//结束计算LTG
			int num_node = idx.size();//LTG点数目
			printf("num_node:%d\n",num_node);
			//imshow("images", imgHeader);
			//cvWaitKey(1);
			if(fabs( m_F - sensor_range) < ZERO_DOUBLE )//目标方向上无障碍
			{
				//fprintf(fp,"i_d: %d\n",i_d);
				best_angle = i_d;
			}
			else//目标方向上有障碍
			{
				//fprintf(fp,"num_node: %d\n",num_node);
				if(num_node > 0)//找到LTG
				{
					printf("num_node:%d\n",num_node);
					int min_distance = 0;//用于记录最小偏差
					int best_i = 0;
					int temp = 0;
					for(int i = 0; i < num_node; i++)
					{
						//计算与目标方向最一致的方向
						temp = abs(idx[i]-n/2);
						if(i == 0)
						{
							min_distance = temp;
						}
						else
						{
							if(min_distance > temp  )
							{
								best_i = idx[i];
								min_distance = temp;
							}
						}
					}
					best_angle = best_i;
					//fprintf(fp,"best_i: %d\n",best_i);
				}
				else//这种情况不可能出现
				{
					printf("num_node = 0\n");
					best_angle = 0.0;
				}//结束未发生碰撞时找到LTG
			}//结束未发生碰撞时目标方向有障碍物
		}//结束未发生碰撞
		else//发生碰撞
		{
			double theta= ComputeTheta();
			if (theta < 90 )
			{
				best_angle = 0;
			}
			else
			{
				best_angle = 319;
			}
			//break;
			//计算自由方向弧
			/*vector<FreeDirectionNode> freeDirectionArcs;
			vector<CollisionDirectionNode> collisionDirectionArcs;
			bool isFirst = true;
			for(int i = 0,j = 0; j < n; j++)
			{
				if(abs(freeDirectionPoints[(j+1)%n] - freeDirectionPoints[j]) == 1)
				{
					if(freeDirectionPoints[j] == 0)
					{
						FreeDirectionNode fdnode={i,j};
						freeDirectionArcs.push_back(fdnode);
					}
					else
					{
						CollisionDirectionNode cdnode={i,j};
						collisionDirectionArcs.push_back(cdnode);
					}
					i = (j+1)%n;
				}
			}
			int num_freeDirectionArcs = freeDirectionArcs.size();
			if(num_freeDirectionArcs == 0)
			{
				printf("num_freeDirectionArcs = 0\n");
				motion.setWalkTargetVelocity(0.0f,0.0f,0.0f,0.5f);
				continue;
			}
			else//存在多条自由弧
			{
				printf("collisionDirectionArcs.size = %d\n",collisionDirectionArcs.size());
				if (collisionDirectionArcs.size() == 0)
				{
					printf("collisionDirectionArcs.size = 0\n");
				}
				else if (collisionDirectionArcs.size() == 1)
				{
					if (obstacles[collisionDirectionArcs[0].start] < obstacles[collisionDirectionArcs[0].end])//右边线
					{
						//best_angle=0;
						motion.setWalkTargetVelocity(0.0f,0.0f,0.5,0.5f);
						continue;
					}
					else//左边线
					{
						motion.setWalkTargetVelocity(0.0f,0.0f,-0.5,0.5f);
						continue;
						//best_angle=319;
					}
				}
			}
			//	memset(buf_step,0,256);
			//	sprintf(buf_step,"images\\%d.jpg",step++);
			//	imwrite(buf_step, imgHeader);

			//if(!TargetInRange)
			//{
				double reachFollow;
				int min_i = 0;
				bool FOUND = false;
				//在自由区域中扫描LTG得到最近点
				int i_d = n/2;
				double m_F = obstacles[i_d%n];//目标方向上的自由距离
				double m_d = 5.0f;//当前机器人与目标的距离
				vector<int> idx;//存放交叉或连续方向
				bool isInsert = false;
				for(int i = 0; i < n ;isInsert = false,i++)
				{
					if(fabs(obstacles[i] - 1.0) < ZERO_DOUBLE &&//剔除探测边界点
						fabs(obstacles[(i+1)%n]-1.0) < ZERO_DOUBLE && 
						fabs(obstacles[(i-1+n)%n]-1.0) < ZERO_DOUBLE )
					{
						isInsert = true;
					}
					if(!isInsert && fabs(obstacles[i] - 1.0) > ZERO_DOUBLE && 
						(fabs(obstacles[(i+1)%n]-obstacles[i]) > 0.2 || 
						fabs(obstacles[(i-1+n)%n]-obstacles[i]) > 0.2))//检测不连续
					{
						idx.push_back(i);
						isInsert = true;
					}
					if(!isInsert && (fabs(obstacles[i] - 1.0) < ZERO_DOUBLE))//检测与障碍物的交点
					{
						bool flag_min = true;//标记是否逐渐变小
						for(int j = 0; j < 5 && flag_min; j++)//假设连续10个变小就表示逐渐变小
						{
							if(obstacles[(i+1+j)%n] > obstacles[(i+j)%n])
								flag_min = false;
						}
						if(flag_min)
						{
							idx.push_back(i);
							isInsert = true;
						}
					}
					if(!isInsert && (fabs(obstacles[i%n] - 1.0) < ZERO_DOUBLE))
					{
						bool flag_max = true;//标记是否逐渐变大
						for(int j = -4; j < 1 && flag_max; j++)//假设前面连续5个逐渐变大
						{
							if(obstacles[(i+j+n)%n] > obstacles[(i+j-1+n)%n])
								flag_max = false;
						}
						if(flag_max)
						{
							idx.push_back(i);
						}
					}
				}//结束计算LTG
				int num_node = idx.size();//LTG点数目

				if(num_node > 0)//找到LTG
				{
					int min_distance = 0;
					int best_i = 0;//保存最好的方向
					int temp = 0;
					//fprintf(fp,"start: %d\n",num_node);
					for(int i = 0; i < num_node; i++)
					{
						//计算最小距离
						temp = abs(idx[i]-n/2);
						if(i == 0)
						{
							min_distance = temp;
						}
						else
						{
							if(min_distance > temp )
							{
								best_i = idx[i];
								min_distance = temp;
							}
						}	
					}//扫描完LTG

					//reachFollow = min_distance;
					if((freeDirectionArcs[0].start)%n < freeDirectionArcs[0].end &&
						best_i > (freeDirectionArcs[0].start)%n &&
						best_i < freeDirectionArcs[0].end )// &&
						//abs(best_i-i_jiyi) < 40)//在自由区域方向上
					{
						min_i = best_i;
						FOUND = true;
						//reachMin = reachFollow;
					}
					if((freeDirectionArcs[0].start)%n > freeDirectionArcs[0].end &&
						(best_i > (freeDirectionArcs[0].start)%n ||
						best_i < freeDirectionArcs[0].end))//&&
						//abs(best_i-i_jiyi) < 40)
					{
						min_i = best_i;
						FOUND = true;
						//reachMin = reachFollow;
					}
				}
				else//这种情况不可能出现
				{
				}//结束未发生碰撞时找到LTG*/
	
	/*			if(FOUND)//找到了较小点
				{
					//fprintf(fp,"reachMin:%lf,min_i:%d\n",reachMin,min_i);
					//if(reachMin < 0.001)
					//TargetInRange = true;
					//朝这个点移动
					best_angle = min_i;
					//best_Speed = 1.0;
					mode = true;//切换为直行
					//擦除相对位置记录
					//	rel_x = 0.0;
					//	rel_y = 0.0;
				}
				else
				{
					//fprintf(fp,"not found,reachMin:%lf\n",reachMin);
					mode = false;//标记为绕行
					int i_best = 0;

					//计算绕行方向
					//往左边走的策略
					i_best = (freeDirectionArcs[0].start+30)%n;

					//基于绕行方向选择函数的方法
			    	int left=-1,right=-1;
					double dis_left=0.0,dis_right=0.0;
					int freedirection ;
					if(freeDirectionArcs[0].start < freeDirectionArcs[0].end)
					{
						freedirection = (freeDirectionArcs[0].start+freeDirectionArcs[0].end)/2;
						if(freedirection >=91 && freedirection < 181)//自由方向向上
						{
							//寻找最近可行区域
							bool IsFindD = false;
							for(int i = freedirection; i > 90 && right == -1; i--)//找右边
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i])>0.2)
								{
									right  = i;
								}
							}
								for(int i = freedirection; i < 181 && left == -1; i++)//找左边
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i])>0.2)
								{
									left  = i;
								}
							}
							if(left == right )
							{
								//墙在右边走固定策略
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else
							{
								if(left == -1 && right !=-1)
								{ 
									//墙在右边
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
								else if(left != -1 && right ==-1)
								{
									//墙在左边
									i_best = (freeDirectionArcs[0].end -30+n)%n;
								}
								else//暂不讨论
								{
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
							}

						}
						else//自由方向向下
						{
							//fprintf(fp,"freeStart:%d,freeEnd:%d\n",freeDirectionArcs[0].start,freeDirectionArcs[0].end);
							//寻找可行区域
							for(int i =  freeDirectionArcs[0].end; i > 90&& right ==-1 ; i--)
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i])>0.2)
								{
									right  = i;
								}
							}
							for(int i =  freeDirectionArcs[0].start; i < 181&& left == -1; i++)
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i])>0.2)
								{
									left  = i;
								}
							}
							if(left == right )
							{
								//墙在右边走固定策略
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else if(left != -1 && right == -1)
							{
								//墙在右边
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else if (left == -1 && right != -1)
							{
								//墙在左边
								i_best = (freeDirectionArcs[0].end-30+n)%n;
							}
							else
							{
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
						}
					}//s<e
					else
					{
						freedirection = (freeDirectionArcs[0].start+freeDirectionArcs[0].end+n)/2%n;
						if(freedirection >=91 && freedirection < 181)//自由方向向上
						{
							for(int i = freedirection; i > 90 && right == -1; i--)//找右边
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i%n])>0.2)
								{
									right  = i;
								}
							}
							for(int i = freedirection; i < 181 && left == -1; i++)//找左边
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i%n])>0.2)
								{
									left  = i;
								}
							}
							if(left == right )
							{
								//墙在右边走固定策略
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else
							{
								if(left == -1 && right !=-1)
								{
									//墙在右边
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
								else if(left != -1 && right ==-1)
								{
									//墙在左边
									i_best = (freeDirectionArcs[0].end - 30+n)%n;
								}
								else
								{
									i_best = (freeDirectionArcs[0].start+30)%n;
								}
							}
						}//end 自由方向向上
						else
						{
							//	fprintf(fp,"start:%d,end:%d\n",freeDirectionArcs[0].start,freeDirectionArcs[0].end);
							int x = freeDirectionArcs[0].end;
							for(int i =  freeDirectionArcs[0].end+20; i >= 70 && right == -1; i--)
							{
								//找间断点
								if(fabs(obstacles[(i-1+n)%n]-obstacles[i%n])>0.2)
								{
									right  = i%n;
								}
							}
							for(int i =  freeDirectionArcs[0].start; i < 160 && left == -1 ; i++)
							{
								//找间断点
								if(fabs(obstacles[(i+1)%n]-obstacles[i%n])>0.2)
								{
									left  = i;
								}
							}
							if(left == right)
							{
								//墙在右边
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
							else if(left == -1 && right != -1)
							{
								//墙在左边
								i_best = (freeDirectionArcs[0].end-30+n)%n;
							}
							else
							{
								//墙在右边
								i_best = (freeDirectionArcs[0].start+30)%n;
							}
						}//end 方向向下
					}//end  s>e
					best_angle = i_best;
				}//end FOUND绕行*/
			//}
		}//end 发生碰撞
		//	fclose(fp);
		if(best_angle <0 || best_angle >=320)
			best_angle  = 0;
		//best_angle = 160;
		desiredDirection = 60/360.0*M_2PI+best_angle*60/360.0*M_2PI/320;
		//desiredSpeed = 0.01;
		//记忆绕行方向
		//i_jiyi = best_angle;
		//3.运动
		//double desiredDirection = 60/360.0*M_2PI+160*60/360.0*M_2PI/320;
		motion.setWalkTargetVelocity(0.4*sin(desiredDirection),0.5*cos(desiredDirection),0.0f,0.5);
		//motion.setWalkTargetVelocity(0.0,0.0,-0.5,0.5f);
		//motion.moveTo(sin(desiredDirection),cos(desiredDirection),0.0f);
		//motion.setWalkTargetVelocity(0.1,sin(desiredDirection)*0.0,0.0f,0.5);
	}

	motion.setWalkTargetVelocity(0,0,0.0f,0.5);
	//	fclose(fp);
	camProxy.unsubscribe(clientName);
	stiffnessLists = 0.0f;
//	motion.stiffnessInterpolation(names, stiffnessLists, timeLists);
	//cout<<"end"<<endl;
}

测试代码 

//tools.cpp
#include "common.h"

void testBlue(Mat&I)
{
	MatIterator_<Vec3b> it, end;
	for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); it != end; ++it)
	{
		if (((*it)[0]< 190 &&  (*it)[0] > 75 )|| ((*it)[1] < 200 && (*it)[1] > 80)||((*it)[2] < 80 && (*it)[2] > 30))
		{
			(*it)[0] = 0;
			(*it)[1] = 0;
			(*it)[2] = 0;
		}
	}
}

double ComputeTheta()//计算边线theta
{
	// 寻找最近两个点
	double min_distance = obstacles2[0].distance;
	int i_min = 0;
	for (int i = 1; i < 319; i++)
	{
		if ( (obstacles2[i].flag == 0 && obstacles2[i].flag == 0)//都为障碍物
			&& fabs(obstacles2[i].distance - obstacles2[(i+1)].distance) < ZERO_CONTINE && obstacles2[i].distance < min_distance)
		{
			min_distance = obstacles2[i].distance;
			i_min = i;
		}
	}
	cout<<"i_min:"<<i_min<<endl;
	// 用i_min和i_min+1计算K
	double i_x,i_y,i1_x,i1_y,theta_t;
	theta_t = M_PI/3+M_PI/3/320*i_min;
	i_x = obstacles2[i_min].distance*cos(theta_t);
	i_y = obstacles2[i_min].distance*sin(theta_t);
	theta_t = M_PI/3+M_PI/3/320*(i_min+1);
	i1_x = obstacles2[i_min+1].distance*cos(theta_t);
	i1_y = obstacles2[i_min+1].distance*sin(theta_t);
	if(atan2(i1_y-i_y,i1_x-i_x)*180/M_PI < ZERO_DOUBLE)
		return atan2(i1_y-i_y,i1_x-i_x)*180/M_PI+180;
	else
		return atan2(i1_y-i_y,i1_x-i_x)*180/M_PI;

}

int ComputeAverage()
{
	int n = 0;;
	for (int i = 1; i < 319; i++)
	{
		if ( (obstacles2[i].flag == 0 && obstacles2[i].flag == 0)//都为障碍物
			&& fabs(obstacles2[i].distance - obstacles2[(i+1)].distance) < ZERO_CONTINE )
		{
			n++;
		}
	}
	return 0;
}

void testMouseAndHandle()
{
	img = imread("rx_images\\22.jpg");
	imshow("src",img);
	//cout<<"rows:"<<img.rows<<"cols:"<<img.cols<<endl;
	setMouseCallback("src", on_mouse, 0 );
	Mat2Array(img);
	//blur(img,img,Size(3,3));
	imshow("changed",img);
	//cout<<ComputeTheta()<<endl;
	//setMouseCallback("src", on_mouse, 0 );
	while(cvWaitKey(10) != 27)
	{
		imshow("changed",img);
	}
}

void testMove()
{
	AL::ALMotionProxy motion("192.168.1.81", 9559);
	const std::string names = "Body";
	float stiffnessLists = 1.0f;
	float timeLists      = 1.0f;
	motion.stiffnessInterpolation(names, stiffnessLists, timeLists);
		char c  = 0;
	clock_t start=clock();
	cvNamedWindow("test");
	motion.setWalkTargetVelocity(0.0f,0.4f,0.0f,0.3f);
	while(c!=27)
	{
		c=cvWaitKey(1);
	}
	clock_t end = clock();
	FILE* fp = fopen("time.txt","w+");
	fprintf(fp,"%d\n",end-start);
	fclose(fp);
	motion.setWalkTargetVelocity(0.0f,0.0f,0.0f,0.3f);

}

void testAction()
{
	AL::ALMotionProxy motion("192.168.1.103", 9559);
	const std::string names = "Body";
	float stiffnessLists = 1.0f;
	float timeLists      = 1.0f;
	motion.stiffnessInterpolation(names, stiffnessLists, timeLists);
	motion.setWalkTargetVelocity(0.2f,0.2f,0.0f,0.5f);
	while(1);
	stiffnessLists = 0.0f;
	motion.stiffnessInterpolation(names, stiffnessLists, timeLists);
}


Mat& ScanImageAndReduceIterator(Mat& I, const uchar* const table)
{
	// accept only char type matrices
	CV_Assert(I.depth() != sizeof(uchar));
	const int channels = I.channels();
	switch(channels)
	{
	case 1:
		{
			MatIterator_<uchar> it, end;
			for( it = I.begin<uchar>(), end = I.end<uchar>(); it != end; ++it)
				*it = table[*it];
			break;
		}
	case 3:
		{
			MatIterator_<Vec3b> it, end;
			for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); it != end; ++it)
			{
				(*it)[0] = table[(*it)[0]];
				(*it)[1] = table[(*it)[1]];
				(*it)[2] = table[(*it)[2]];
			}
		}
	}
	return I;
}

Mat& TestNotGreen(Mat& I)
{
	CV_Assert(I.depth() != sizeof(uchar));
	const int channels = I.channels();
	switch(channels)
	{
	case 1:
		{
			MatIterator_<uchar> it, end;
			for( it = I.begin<uchar>(), end = I.end<uchar>(); it != end; ++it)
				if(*it != 255) *it = 0;
			break;
		}
	case 3:
		{
			MatIterator_<Vec3b> it, end;
			for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); it != end; ++it)
			{
				if (((*it)[0]< 190 &&  (*it)[0] > 75 )|| ((*it)[1] < 200 && (*it)[1] > 80)||((*it)[2] < 80 && (*it)[2] > 30))
				{
					(*it)[0] = 0;
					(*it)[1] = 0;
					(*it)[2] = 0;
				}
			}
		}
	}
	blur(I,I,Size(3,3));
	return I;
}

Mat& TestWhite(Mat& I)
{
	// accept only char type matrices
	CV_Assert(I.depth() != sizeof(uchar));
	const int channels = I.channels();
	switch(channels)
	{
	case 1:
		{
			MatIterator_<uchar> it, end;
			for( it = I.begin<uchar>(), end = I.end<uchar>(); it != end; ++it)
				if(*it != 255) *it = 0;
			break;
		}
	case 3:
		{
			MatIterator_<Vec3b> it, end;
			for( it = I.begin<Vec3b>(), end = I.end<Vec3b>(); it != end; ++it)
			{
				if ((*it)[0]< 235 || (*it)[1] < 235 ||(*it)[2] < 235)
				{
					(*it)[0] = 0;
					(*it)[1] = 0;
					(*it)[2] = 0;
				}
			}
		}
	}
	return I;
}

int computePixel(double theta)
{
	double m = 1.0/tan(theta-60.0/360*2*3.1415926)*sin(60.97/360*2*3.1415926)-cos(60.97/360*2*3.1415926);
	return (320-int(320.0/(1+m)))%320;
}

/************************************************************************/
/* name:   
/* purpose: 利用图像得到距离机器人的扇形区域内障碍物的角度和距离，将结果放在obstacles数组中,并且把原图像也改变
/* paramter: 图像矩阵
/* return:                         6                                        */
/************************************************************************/
void Mat2Array(cv::Mat& img)
{
	//检测非绿色
	//TestNotGreen(img);
	//检测白色
	//TestWhite(img);
	//imshow("white",img);
	//平滑滤波
//	blur(img,img,Size(3,3));
	//转换成角度和距离，从底部开始扫描到100
	double k0 = 60/360.0*6.28,k=0.0;
	double incr =60/360.0*6.28/320;
	for (int i = 0; i < 320; i++)
	{
		k = k0+i*incr;
		int x = computePixel(k);
		bool flag = false;
		for (int j=239; j>=100 && !flag;j--)
		{
			uchar b = *(img.data + img.step[0] * j + img.step[1] * x);
			uchar g =*(img.data + img.step[0] * j + img.step[1] * x+1);
			uchar r =*(img.data + img.step[0] * j + img.step[1] * x+2);
			if(b< 235  || g < 235  || r < 235 )//非白色
			{
				if ((b< 235 && b >110) || (g < 235 && g > 105) || (r < 235 && r > 85))//绿色地毯
				{
					double k = 100/240.0;
					double theta = atan(0.3/0.48)*360/6.28;
					double shang = k*sin(theta/360*6.28)+(1-k)*sin((47.64+theta)/360*6.28);
					double xia = k*cos(theta/360*6.28)+(1-k)*cos((47.64+theta)/360*6.28);
					obstacles2[i].flag = -1;//绿色
					obstacles2[i].distance= 0.48*shang/xia*pow(M_E,fabs(k-1.0));
					*(img.data + img.step[0] * j + img.step[1] * x) = 0;
					*(img.data + img.step[0] * j + img.step[1] * x+1)=255;
					*(img.data + img.step[0] * j + img.step[1] * x+2)=0;
				}
				else//蓝色障碍物
				{
					double k = j/240.0;
					double theta = atan(0.3/0.48)*360/6.28;
					double shang = k*sin(theta/360*6.28)+(1-k)*sin((47.64+theta)/360*6.28);
					double xia = k*cos(theta/360*6.28)+(1-k)*cos((47.64+theta)/360*6.28);
					obstacles2[i].distance= 0.48*shang/xia*pow(M_E,fabs(k-1.0));
					obstacles2[i].flag = 0;//蓝色
					*(img.data + img.step[0] * j + img.step[1] * x) = 255;
					*(img.data + img.step[0] * j + img.step[1] * x+1)=0;
					*(img.data + img.step[0] * j + img.step[1] * x+2)=0;
					flag = true;
				}
			}
			else//白色边线
			{
				double k = j/240.0;
				double theta = atan(0.3/0.48)*360/6.28;
				double shang = k*sin(theta/360*6.28)+(1-k)*sin((47.64+theta)/360*6.28);
				double xia = k*cos(theta/360*6.28)+(1-k)*cos((47.64+theta)/360*6.28);
				obstacles2[i].distance= 0.48*shang/xia*pow(M_E,fabs(k-1.0));
				obstacles2[i].flag = 1;//白色

				*(img.data + img.step[0] * j + img.step[1] * x) = 255;
				*(img.data + img.step[0] * j + img.step[1] * x+1) =255;
				*(img.data + img.step[0] * j + img.step[1] * x+2) = 255;
				flag = true;
			}
		}
	}
	
	for (int l=0; l < 320; l++)
	{
		printf("%lf\t",obstacles2[l].distance);
	}
	printf("\n");

	// 	//减少颜色空间
	// 	uchar table[256];
	// 	int divideWith=30; 
	//for (int i = 0; i < 256; ++i)
	//	table[i] = divideWith* (i/divideWith);
	// 	ScanImageAndReduceIterator(img,table);

}

void testPixel(Mat &img,int col,int row)
{
 	uchar b = *(img.data + img.step[0] * row + img.step[1] * col);
	uchar g =*(img.data + img.step[0] * row + img.step[1] * col+1);
 	uchar r =*(img.data + img.step[0] * row + img.step[1] * col+2);
//	assert(fp != NULL && "fp is NULL");
	FILE* fp = fopen("color.txt","a+");
	fprintf(fp,"%d,%d,r:%d,g:%d,b:%d\n",img.step[0],img.step[1],r,g,b);
	fclose(fp);
//	fprintf(fp,"%d,%d,r:%d,g:%d,b:%d\n",img.step[0],img.step[1],r,g,b);
}

void on_mouse( int event, int x, int y, int flags, void* ustc)  
{  
	CvFont font;  
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, CV_AA);  
	//cout<<"rows:"<<img.rows<<"cols:"<<img.cols<<endl;
	if( event == CV_EVENT_LBUTTONDOWN )  
	{  
		CvPoint pt = cvPoint(x,y);  
		printf("(x:%d,y:%d)",pt.x,pt.y);
		//cout<<"rows:"<<img.rows<<"cols:"<<img.cols<<endl;
		testPixel(img,x,y);
		//cv::putText(img,temp, pt, FONT_HERSHEY_SIMPLEX, 0.3f, cv::Scalar(255, 255, 255, 0));  
		//cv::circle( img, pt, 2,cv::Scalar(255,0,0,0) ,CV_FILLED, CV_AA, 0 );   
	}
}

主函数 

//test.cpp
#include "common.h"

cv::Mat img;
double obstacles[320];//从60度开始扫描
Dis_flag obstacles2[320];

int main()
{
	try {
		//testMove();
		bz();
		//testMouseAndHandle();
	}
	catch (const AL::ALError& e) {
		std::cerr << "Caught exception: " << e.what() << std::endl;
		exit(1);
	}
	exit(0);
	return 0;
}








zouroot
关注
  • 3
    点赞
  • 踩
  • 25
    收藏
    觉得还不错? 一键收藏
  • 10
    评论
专栏目录
乐高机器人java程序代码_用JAVA编写一个乐高机器人躲避障碍物运动到目标点的程序....
weixin_29660659的博客
02-13 835
写出一个可以控制机器人的小程序,使机器人从一边到一个相对面,并至少跨越一个障碍物.规则如下:1，障碍物必须设置在机器人行走的路线上.2，空间的基本配置如插图3,不能用轨道之类的东西...写出一个可以控制机器人的小程序,使机器人从一边到一个相对面,并至少跨越一个障碍物.规则如下:1，障碍物必须设置在机器人行走的路线上.2，空间的基本配置如插图3, 不能用轨道之类的东西引导机器人.4，机器人可以触碰到...
避障机器人程序c语言,基于51单片机小车寻迹、避障源程序(注释很详细)
weixin_29331015的博客
05-18 2037
/** 作者：赵新* 功能：实现小车躲避障碍，通过检测三个方向的距离，选择最大距离转弯* 日期：2015/3/14* 说明：STC89c52RC,12MHz* 注意：1000ms和100ms待测，完成后删除此行*—————————————————管脚说明——————————————* Trig = P1^0* Echo = P3^2* PWM_OUT = P0^4*————————————————...
【路径规划】基于A_star算法机器人避障最短路径规划附matlab代码
qq_59747472的博客
12-28 278
A*搜索算法,俗称 A‐Star,是一种广泛应用于路径的规划和图形遍历的计算机算法.本文使用该算法对可运动机器人的路径进行智能规划.该算法综合了 Best‐First Search 和 Dijkstra 算法的优点,在进行启发式搜索提高算法效率的同时,可以保证找到一条最优路径.最后本文使用栅格法建立路径模型,最终利用 MATLAB对该算法进行了仿真,证明了 A‐Star 能够为机器人寻找一条有效的从起始点到终止点的最佳路径.
【动态规划】基于A星算法实现机器人动态避障附matlab代码
最新发布
m0_57702748的博客
04-26 462
✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。????个人主页：Matlab科研工作室????个人信条：格物致知。更多Matlab仿真内容点击????智能优化算法 神经网络预测 雷达通信 无线传感器 电力系统信号处理 ...
【路径规划】基于DWA实现机器人动态避障附matlab代码
matlab_dingdang的博客
09-14 1981
DWA 算法是基于机器人运动学与动力学理论的一种局部避障算法，它将对机器人的位置控制转换为对机器人的速度控制。DWA 算法可以概括为三步:一是根据机器人自身的限制以及环境制约将速度的采样空间约束在一定范围内;二是根据机器人运动学对采样后的速度进行模拟得到预轨迹;三是设定评价函数对预轨迹进行评分以获取最优轨迹对应的执行速度。智能优化算法。
NAO机器人避障代码
12-20
适合于NAO机器人，使用Python编写，下载下来改一下机器人的IP就能用。 程序功能：调用机器人的声呐模块，识别一定距离内是否有障碍物，根据逻辑关系作出相应的动作。
nao机器人接力赛代码
03-11
在本文中，我们将深入探讨"nao机器人接力赛代码"这一主题，这是一份用于2018年江苏省大学生机器人比赛的关键程序。NAO机器人，由Aldebaran Robotics（现Softbank Robotics）开发，是一种先进的人形机器人，广泛应用...
NAO 机器人舞蹈程序，nao 机器人大王叫我来巡山舞蹈程序.zip
05-21
NAO机器人是一款由法国Aldebaran Robotics公司开发的高度先进的社交机器人，被广泛应用于教育、研究和娱乐领域。这款机器人以其灵活的肢体动作和高度的人工智能能力而著名，能够执行各种复杂的任务，包括跳舞。"大王...
nao机器人接力比赛代码.zip
06-20
在机器人接力赛中，白线作为路径指示，NAO机器人需要通过视觉传感器（如摄像头）获取环境信息，然后利用图像处理算法，如边缘检测或颜色分割，来识别白线并确定自身的位置和方向。代码中可能包含了这部分功能的实现...
Webots平台NAO机器人寻路避障实现附件
01-21
此外，文件列表中的"NAO"可能是场景配置文件或者代码文件，它可能包含了NAO机器人的模型定义、初始配置以及避障算法的具体实现。打开并分析这些文件，将有助于我们更好地理解和学习这个项目。 总之，通过Webots平台...
机器人避障matlab代码.rar
07-28
机器人避障matlab代码.rar
机器人避障算法的matlab仿真代码
04-05
这是一个学习机器人避障算法的matlab仿真代码，直接下载运行就可以使用，备注非常详细，大家可以自定义避障地图，也可以更改参数实现不同的避障
移动机器人运动规划避障的一些算法代码
10-29
移动机器人的避障和运动规划算法代码，包括D算法A*算法基于模糊控制的算法等
智能寻迹避障机器人
01-13
智能寻迹避障机器人
NAO机器人
小琳猫的博客
04-12 3119
本篇博客参考清华大学出版社出版的《NAO机器人程序设计》一书，对其中的部分内容进行总结，以便加深理解和记忆
nao机器人国际比赛程序 python编写
热门推荐
zouroot的专栏
02-01 1万+
nao机器人国际比赛，程序公开，里面涉及到了nao机器人捡垃圾、蹲下、阶梯跑、踢球等程序，可以参考学习！采用python语言编写。
【毕业设计】23-基于单片机的自动避障机器人设计（原理图+源代码+仿真+论文+答辩PPT）
qq_22592979的博客
11-29 1183
本文主要研究内容为基于单片机的自动避障机器人。避障机器人是一种采用了多种传感器的自动控制系统。系统采用红外检测障碍物，一旦检测到前方有障碍物则将信号发送给单片机，通过单片机处理分析，将控制信息输出到电机驱动控制机器人能够顺利有效的避开障碍物。本系统以AT89C51单片机为核心，红外检测器模块为传感器，385直流电机及电机驱动L298N为被控制对象控制机器人躲避障碍物。具体的避障流程为：系统开始运行，首先是直行，当红外检测器探测到有右端有障碍物后控制电机左轮速度慢于右轮，实现左转。同理左端检测到障碍物后控制电
避障机器人程序c语言,移动机器人的避障实验设计+源程序+流程图.doc
weixin_32926649的博客
05-18 2032
移动机器人的避障实验设计源程序流程图移动机器人的避障实验设计+源程序+流程图摘要:随着科学技术的日益，机器人越来越融入到人们的生活。近年来，特别是智能机器人的开发与研究引起了很多学者的关注。其中，机器人的避障问题成为了机器人研究的热点。传统的避障方法如可视图法、栅格法、自由空间法等算法可以解决障碍物信息己知时的情况。但在试验条件确定的情况下，很多方法就比较复杂，因此，我根据现有的红外探头进行了简单...
V-REP与Python编程实现NAO机器人避障实验
首先，V-REP是一个功能强大且免费的机器人模拟器，它允许用户设计、构建和测试各种类型的机器人，包括NAO。Python被选择作为控制工具，因为它是一种流行的脚本语言，适合编写自动化程序。Spyder是Python的一个优秀...
评论 10
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值