Demo entry 6754805

ss

   

Submitted by anonymous on Jul 22, 2018 at 10:55
Language: C++. Code size: 25.1 kB.

#include "ros/ros.h"
//#include "sepf/msgSepf.h"
#include <std_msgs/Empty.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
extern "C"{
	//#include "v_repConst.h"
	//#include "extApiPlatform.h"
	#include "extApi.h"
}

#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <math.h>
#include <tf/transform_broadcaster.h>
#include <angles/angles.h>
#include <pthread.h>
#include <fcntl.h>
#include <unistd.h>
#include <linux/joystick.h>
#include <sys/ioctl.h>

#define JOY_DEV "/dev/input/js0"

#define numP 4000
#define numP3 12000
#define PI 3.14159265358979323846
#define joyMAX 32767
#define joyLimit 1500
char jsbuttons[8];
int jsaxes[8];

class Sepf
{
public:
	Sepf()
	{
		printf("Start init\n");
		//takeOffPubName = "/bebop/takeoff";
		//landPubName = "/bebop/land";
		//resetPubName = "/bebop/reset";
		//cmdPubName = "/bebop/cmd_vel";
		cmdPubName = "/VREP/cmd_vel";
		vrepDronePoseName = "/VREP/drone_pos";
		vrepDroneVelName = "/VREP/drone_vel";
		vrepDObject1PoseName = "/VREP/DCylinder_pos";
		vrepDObject1VelName = "/VREP/DCylinder_vel";
		vrepDObject2PoseName = "/VREP/DCuboid_pos";
		vrepDObject2VelName = "/VREP/DCuboid_vel";
		//cmdPubCName = "/vservo/cmd_vel";
		//optiR1SubName = "/Robot_1/pose";
		//bebopconSubName = "/vel_ctrl/cmd_vel";
		//takeOffPub = nh.advertise<std_msgs::Empty>("/bebop/takeoff", 100);
		//landPub = nh.advertise<std_msgs::Empty>("/bebop/land", 100);
		//resetPub = nh.advertise<std_msgs::Empty>("/bebop/reset", 100);
		
		cmdPub = nh.advertise<geometry_msgs::Twist>(cmdPubName, 100);
		//cmdPubC = nh.advertise<geometry_msgs::Twist>("/vservo/cmd_vel", 100);

		if(initVREP() == -1)
		{
			printf("Error connect vrep\n");
		}
		else
		{
			printf("Connect VREP\n");
		}

		//optiR1CurData = *(ros::topic::waitForMessage<geometry_msgs::PoseStamped>(optiR1SubName));
		//printf("connect Optitrack\n");


		a_fo = 2;
		b_fo = 0.5;
		a_fi = 0.5;
		b_fi = 0.5;
		a_bi = 0.5;
		for(i=0; i< numP3; i++)
		{
			pointCloud[i] = 0;
			avoidList[i] = 0;
			avoidList2[i] = 0;
		}
		for(i=0; i< 3; i++)
		{
			dronePos[i] = 0;
			droneOri[i] = 0;
			droneVel[i] = 0;
			v_in[i] = 0;
			v_ref[i] = 0;
			v_ref_cv[i] = 0;
			max_ca[i] = 0;
			max_ca2[i] = 0;
			min_ca[i] = 0;
			min_ca2[i] = 0;
			v_ca[i] = 0;
			v_ca2[i] = 0;
		}
		droneVelNorm = 0;

		setDroneVel = 0.5;
		setDroneVelGap = 0.1;
		setDroneAngular = 0;
		a_max = 1;
		n = 4;
		t_f = 1;
		tflag = 1;

		preDiff = 0;
		currentDiff = 0;
		preDist = 0;
		currentDist = 0;
		numObject = 2;

		//optiSub = nh.subscribe(optiR1SubName, 10, &Sepf::optimsgCallback, this);
		//bebopSub = nh.subscribe(bebopconSubName, 10, &Sepf::bebopmsgCallback, this);
		vrepDronePose = nh.subscribe(vrepDronePoseName, 10, &Sepf::vrepDronePosCallback, this);
		vrepDroneVel = nh.subscribe(vrepDroneVelName, 10, &Sepf::vrepDroneVelCallback, this);
		vrepDObject1Pose = nh.subscribe(vrepDObject1PoseName, 10, &Sepf::vrepDObject1PosCallback, this);
		vrepDObject1Vel = nh.subscribe(vrepDObject1VelName, 10, &Sepf::vrepDObject1VelCallback, this);
		vrepDObject2Pose = nh.subscribe(vrepDObject2PoseName, 10, &Sepf::vrepDObject2PosCallback, this);
		vrepDObject2Vel = nh.subscribe(vrepDObject2VelName, 10, &Sepf::vrepDObject2VelCallback, this);
		printf("set sub\n");
	};
	

	void vrepDronePosCallback(const geometry_msgs::PoseStamped::ConstPtr& dronePosData)
	{
		dronePos[0] = dronePosData->pose.position.x;
		dronePos[1] = dronePosData->pose.position.y;
		dronePos[2] = dronePosData->pose.position.z;
		tf::quaternionMsgToTF(dronePosData->pose.orientation, quater);
		tf::Matrix3x3(quater).getRPY(droneOri[0], droneOri[1], droneOri[2]);
		
	};

	void vrepDroneVelCallback(const geometry_msgs::TwistStamped::ConstPtr& droneVelData)
	{
		droneVel[0] = droneVelData->twist.linear.x;
		droneVel[1] = droneVelData->twist.linear.y;
		droneVel[2] = droneVelData->twist.linear.z;
	};

	void vrepDObject1PosCallback(const geometry_msgs::PoseStamped::ConstPtr& objectPosData)
	{
		dObjectPos[0][0] = objectPosData->pose.position.x;
		dObjectPos[0][1] = objectPosData->pose.position.y;
		dObjectPos[0][2] = objectPosData->pose.position.z;
	};

	void vrepDObject1VelCallback(const geometry_msgs::TwistStamped::ConstPtr& objectVelData)
	{
		dObjectVel[0][0] = objectVelData->twist.linear.x;
		dObjectVel[0][1] = objectVelData->twist.linear.y;
		dObjectVel[0][2] = objectVelData->twist.linear.z;
	};

	void vrepDObject2PosCallback(const geometry_msgs::PoseStamped::ConstPtr& objectPosData)
	{
		dObjectPos[1][0] = objectPosData->pose.position.x;
		dObjectPos[1][1] = objectPosData->pose.position.y;
		dObjectPos[1][2] = objectPosData->pose.position.z;
	};

	void vrepDObject2VelCallback(const geometry_msgs::TwistStamped::ConstPtr& objectVelData)
	{
		dObjectVel[1][0] = objectVelData->twist.linear.x;
		dObjectVel[1][1] = objectVelData->twist.linear.y;
		dObjectVel[1][2] = objectVelData->twist.linear.z;
	};

	/*
	void optimsgCallback(const geometry_msgs::PoseStamped::ConstPtr& optiR1Data)
	{
		
		optiR1CurData = *optiR1Data;
		dronePos[0] = optiR1Data->pose.position.x;
		dronePos[1] = optiR1Data->pose.position.y;
		dronePos[2] = optiR1Data->pose.position.z;
	};
	
	void bebopmsgCallback(const geometry_msgs::Twist::ConstPtr& bebopVelData)
	{
		cmdMsg.linear.x = bebopVelData->linear.x;
		cmdMsg.linear.y = bebopVelData->linear.y;
		cmdMsg.linear.z = bebopVelData->linear.z;
		cmdMsg.angular.z = bebopVelData->angular.z;
		cmdPub.publish(cmdMsg);
	};
	*/


public:
	int initVREP()
	{
		vrepClientID = simxStart((simxChar*) "127.0.0.1", (simxInt)19997, (simxUChar)true, (simxUChar)true, (simxInt)5000, (simxInt)5);
		return vrepClientID;
	};

	void sepf2D()
	{
		//printf("start sepf2D\n");

		for(i=0; i<3; i++)
		{
			v_ref[i] = 0;
			v_ref_cv[i] = 0;
			max_ca[i] = 0;
			max_ca2[i] = 0;
			min_ca[i] = 0;
			min_ca2[i] = 0;
			v_ca[i] = 0;
			v_ca2[i] = 0;
		}
		
		cmdMsg.linear.x = 0;
		cmdMsg.linear.y = 0;
		cmdMsg.linear.z = 0;
		cmdMsg.angular.z = 0;
		
		/*
		cmdCMsg.linear.x = 0;
		cmdCMsg.linear.y = 0;
		cmdCMsg.linear.z = 0;
		cmdCMsg.angular.z = 0;
		*/
		/*
		tf::quaternionMsgToTF(optiR1CurData.pose.orientation, quater);
		tf::Matrix3x3(quater).getRPY(droneOri[0], droneOri[1], droneOri[2]);
		//droneOri[2] = angles::normalize_angle_positive(droneOri[2]);
		//printf("%f %f %f\n", droneOri[0], droneOri[1], droneOri[2]);
		at = optiR1PreData.header.stamp.sec;
		ant = optiR1PreData.header.stamp.nsec;
		bt = optiR1CurData.header.stamp.sec;
		bnt = optiR1CurData.header.stamp.nsec;
		div_time = (bt-at) * 1000000000 + (bnt - ant);
		div_time = div_time / 1000000000.0;
		ax = optiR1CurData.pose.position.x;
		ay = optiR1CurData.pose.position.y;
		az = optiR1CurData.pose.position.z;
		bx = optiR1PreData.pose.position.x;
		by = optiR1PreData.pose.position.y;
		bz = optiR1PreData.pose.position.z;
		droneVel[0] = (bx-ax) / div_time;
		droneVel[1] = (by-ay) / div_time;
		droneVel[2] = (by-az) / div_time;
		optiR1PreData = optiR1CurData;
		*/


		double v_local_in[3] = {0, };
		
		if(jsaxes[4] > joyLimit || jsaxes[4] < -joyLimit)
			v_local_in[0] = -((double)jsaxes[4] / joyMAX * setDroneVel);
		if(jsaxes[3] > joyLimit || jsaxes[3] < -joyLimit)
			v_local_in[1] = -((double)jsaxes[3] / joyMAX * setDroneVel);
		
		if(jsaxes[1] > joyMAX/2)
			v_local_in[2] = 0.5;
		else if(jsaxes[1] < -joyMAX/2)
			v_local_in[2] = -0.5;
		else
			v_local_in[2] = 0;

		if(jsaxes[0] > joyMAX/2)
			setDroneAngular = -0.3;
		else if(jsaxes[0] < -joyMAX/2)
			setDroneAngular = 0.3;
		else
			setDroneAngular = 0;
		//printf("%f %f %f\n", v_local_in[0], v_local_in[1], v_local_in[2]);
		//v_in[0] = v_local_in[0];
		//v_in[1] = v_local_in[1];
		//v_in[2] = v_local_in[2];
		
		v_in[0] = -v_local_in[1] * sin(droneOri[2]) + v_local_in[0] * cos(droneOri[2]);
		v_in[1] = +v_local_in[1] * cos(droneOri[2]) + v_local_in[0] * sin(droneOri[2]);
		v_in[2] = v_local_in[2];
		
		//printf("%f %f %f\n", v_in[0], v_in[1], v_in[2]);
		

		for(i=0; i< numP3; i++)
		{
			avoidList[i] = 0;
			avoidList2[i] = 0;
		}


		//Read vrep LiDAR
		int errorCode = 0;
		simxUChar* signalData;
		simxInt signalLen;
		errorCode = simxGetStringSignal(vrepClientID, "velcloud", &signalData, &signalLen, simx_opmode_oneshot_wait);
		if(errorCode == 0)
		{
			//Unpacking signal string to floats
			int cnt = signalLen/4;
			int tnP = cnt / 3;
			//printf("cnt: %d\n", cnt);
			for(int vrepi =0; vrepi < cnt; vrepi++)
			{
				pointCloud[vrepi] = ((float*) signalData)[vrepi];
			}

			//Calculate SEPF
			norm_in = sqrt(pow(v_in[0], 2) + pow(v_in[1], 2));
			norm_in2 = sqrt(pow(droneVel[0], 2) + pow(droneVel[1], 2));
			droneVelNorm = norm_in2;
			d_min=droneVelNorm*droneVelNorm / (2*a_max);
			a_fi = a_bi + d_min;

			//printf("after unpacking\n");
			for(i = 0; i < tnP; i++)
			{
				double P_ro[3] = {0, 0, 0};
				double P_ro2[3] = {0, 0, 0};
				double theta, theta2, R_o, R_o2, R_i, R_i2, norm_Pro, norm_Pro2;
				
				//CA1 - user input
				P_ro[0] = pointCloud[i*3] - dronePos[0];
				P_ro[1] = pointCloud[i*3+1] - dronePos[1];
				theta = atan2(P_ro[0], P_ro[1]);
				droneInpAng = atan2(v_in[0], v_in[1]);
				theta = theta -droneInpAng;
				if(theta > PI)
					theta = theta - 2 * PI;
				else if (theta < -PI)
					theta = theta + 2 * PI;

				if(!(theta < -PI/2) && !(theta > PI/2))
				{
					R_o = (a_fo * b_fo) / pow(pow(fabs(a_fo*sin(theta)),n) + pow(fabs(b_fo*cos(theta)),n), 1.0/n);
					R_i = (a_fi * b_fi) / pow(pow(fabs(a_fi*sin(theta)),n) + pow(fabs(b_fi*cos(theta)),n), 1.0/n);

					norm_Pro = sqrt(pow(P_ro[0],2) + pow(P_ro[1],2));
					if(norm_Pro < R_o)
					{

						double v_ca_temp[2] = {0, 0};
						double n_ro[2] = {0,0};
						double P_com;
						double mu_bar;
						P_com = (norm_Pro - R_i)/(R_o - R_i) -1;
						mu_bar = norm_in;
						n_ro[0] = P_ro[0] / norm_Pro;
						n_ro[1] = P_ro[1] / norm_Pro;
						if(norm_Pro < R_i)
						{
							v_ca_temp[0] = mu_bar * n_ro[0];
							v_ca_temp[1] = mu_bar * n_ro[1];
						}
						else
						{
							v_ca_temp[0] = mu_bar * P_com * n_ro[0];
							v_ca_temp[1] = mu_bar * P_com * n_ro[1];
						}
						// if((v_ca_temp[0] > 0 && v_in[0] > 0) || (v_ca_temp[0] < 0 && v_in[0] < 0 ))
						// {
						// 	printf("%f %f / %f %f / %f %f / %f %f / %f\n", v_in[0], v_in[1], droneVel[0], droneVel[1], dronePos[0], dronePos[1], pointCloud[i*3], pointCloud[i*3+1], theta);
						// }
						avoidList[i*3] = v_ca_temp[0];
						avoidList[i*3+1] = v_ca_temp[1];
						//printf("%f %f\n", v_ca_temp[0], v_ca_temp[1]);
					}
				}
				//CA2 - drone movement
				P_ro2[0] = pointCloud[i*3] - dronePos[0];
				P_ro2[1] = pointCloud[i*3+1] - dronePos[1];
				theta2 = atan2(P_ro[0], P_ro[1]);
				droneVelAng = atan2(droneVel[0], droneVel[1]);
				theta2 = theta2 - droneVelAng;
				if(!(theta2 > PI/2 && theta2 < -PI/2))
				{
					R_o2 = (a_fo * b_fo) / pow(pow(fabs(a_fo*sin(theta2)),n) + pow(fabs(b_fo*cos(theta2)),n), 1.0/n);
					R_i2 = (a_fi * b_fi) / pow(pow(fabs(a_fi*sin(theta2)),n) + pow(fabs(b_fi*cos(theta2)),n), 1.0/n);

					norm_Pro2 = sqrt(pow(P_ro2[0],2) + pow(P_ro2[1],2));
					if(norm_Pro2 < R_o2)
					{
						double v_ca_temp[2] = {0, 0};
						double n_ro[2] = {0,0};
						double P_com;
						double mu_bar;
						P_com = (norm_Pro2 - R_i2)/(R_o2 - R_i2) -1;
						mu_bar = norm_in2;
						n_ro[0] = P_ro2[0] / norm_Pro;
						n_ro[1] = P_ro2[1] / norm_Pro;
						if(norm_Pro2 < R_i2)
						{
							v_ca_temp[0] = - mu_bar * n_ro[0];
							v_ca_temp[1] = - mu_bar * n_ro[1];
						}
						else
						{
							v_ca_temp[0] = mu_bar * P_com * n_ro[0];
							v_ca_temp[1] = mu_bar * P_com * n_ro[1];
						}
						avoidList2[i*3] = v_ca_temp[0];
						avoidList2[i*3+1] = v_ca_temp[1];
					}
				}
			}

			//v_ca = maximum positive + minimum negative
			for(i=0; i<tnP; i++)
			{
				double temp = avoidList[i*3];
				if(temp > max_ca[0])
				{
					max_ca[0] = temp;
				}
				else if(temp < min_ca[0])
				{
					min_ca[0] = temp;
				}
				temp = avoidList[i*3+1];
				if(temp > max_ca[1])
				{
					max_ca[1] = temp;
				}
				else if(temp < min_ca[1])
				{
					min_ca[1] = temp;
				}
				temp = avoidList2[i*3];
				if(temp > max_ca2[0])
				{
					max_ca2[0] = temp;
				}
				else if(temp < min_ca2[0])
				{
					min_ca2[0] = temp;
				}
				temp = avoidList2[i*3+1];
				if(temp > max_ca2[1])
				{
					max_ca2[1] = temp;
				}
				else if(temp < min_ca2[1])
				{
					min_ca2[1] = temp;
				}

			}
			//printf("calcul vca2\n");
			if(max_ca[0] < 0)
				max_ca[0] = 0;
			if(max_ca[1] < 0)
				max_ca[1] = 0;
			if(min_ca[0] > 0)
				max_ca[0] = 0;
			if(min_ca[1] > 0)
				max_ca[1] = 0;

			if(max_ca2[0] < 0)
				max_ca2[0] = 0;
			if(max_ca2[1] < 0)
				max_ca2[1] = 0;
			if(min_ca2[0] > 0)
				max_ca2[0] = 0;
			if(min_ca2[1] > 0)
				max_ca2[1] = 0;

			//speed restriction 
			if((fabs(v_in[0]) < fabs(v_ca[0])) && (fabs(v_in[1]) < fabs(v_ca[1])))
			{
				printf("warning\n");
			}

			if((fabs(droneVel[0]) < fabs(v_ca2[0])) && (fabs(droneVel[1]) < fabs(v_ca2[1])))
			{
				printf("warning2\n");
			}

			//Calculate v_ref
			v_ca[0] = max_ca[0] + min_ca[0];
			v_ca[1] = max_ca[1] + min_ca[1];
			v_ca2[0] = max_ca2[0] + min_ca2[0];
			v_ca2[1] = max_ca2[1] + min_ca2[1];
			v_ref[0] = v_in[0] + v_ca[0] + v_ca2[0];
			v_ref[1] = v_in[1] + v_ca[1] + v_ca2[1];

			//Limit drone's maximum velocity (Maximum: user input)

			if(v_ref[0] > setDroneVel)
			{
				v_ref[0] = setDroneVel;
			}
			if(v_ref[1] > setDroneVel)
			{
				v_ref[1] = setDroneVel;
			}
			if(v_ref[0] < -setDroneVel)
			{
				v_ref[0] = -setDroneVel;
			}
			if(v_ref[1] < -setDroneVel)
			{
				v_ref[1] = -setDroneVel;
			}

			//Calculate obj velocity
			//Page 1
			double P_ro[3] = {0, 0, 0};
			double norm_Pro;
			pointIdx=0;
			diffIdx = 0;
			currentDist = 0;
			currentDiff = 0;
			for(i = 0; i < numP; i++)
			{
				P_ro[0] = pointCloud[i*3+0] - dronePos[0];
				P_ro[1] = pointCloud[i*3+1] - dronePos[1];
				norm_Pro = sqrt(pow(P_ro[0],2) + pow(P_ro[1],2));
				currentDist = norm_Pro;
				currentDiff = currentDist - preDist;
				if(fabs(currentDiff) > fabs(preDiff)+0.5)
				{
					diff[diffIdx] = i;
					diffIdx++;
					//printf("%f %f %f %f\n", preDist, currentDist, preDiff, currentDiff);
					if(diffIdx>10)
					{
						printf("warning idx\n");
						break;
					}
				}
				preDiff = currentDiff;
				preDist = currentDist;
			}

			P_ro[0] = pointCloud[0] - dronePos[0];
			P_ro[1] = pointCloud[1] - dronePos[1];
			norm_Pro = sqrt(pow(P_ro[0],2) + pow(P_ro[1],2));
			currentDist = norm_Pro;
			currentDiff = currentDist - preDist;
			if(fabs(currentDiff) > fabs(preDiff)+0.5)
			{
			}
			else
			{
				diffIdx--;
				for(int i = 0; i < diffIdx; i++)
				{
					diff[i] = diff[i+1];

				}
				diff[diffIdx+1] = 0;
			}
			preDiff = currentDiff;
			preDist = currentDist;
			printf("idx: %d / ", diffIdx);
			for(int i = 0; i<idx; i++)
			{
				printf("%d ", diff[i]);
			}
			printf("\n");
			//Page 2
			for(int i = 0; i < diffIdx-1; i++)
			{
				for(int j = diff[i]; j < diff[i+1]; j++)
				{
					objectCloud[i][0] += pointCloud[j*3+0];
					objectCloud[i][1] += pointCloud[j*3+1];
					objectCloud[i][2] += pointCloud[j*3+2];
				}
				
				objectCloud[i][0] /= (diff[i+1] - diff[i]);
				objectCloud[i][1] /= (diff[i+1] - diff[i]);
				objectCloud[i][2] /= (diff[i+1] - diff[i]);
				//printf("%d ",(diff[i+1] - diff[i]));
			}
			
			for(int j = 0; j < diff[0]; j++)
			{
				objectCloud[diffIdx-1][0] = pointCloud[j*3+0];
				objectCloud[diffIdx-1][1] = pointCloud[j*3+1];
				objectCloud[diffIdx-1][2] = pointCloud[j*3+2];
			}
			for(int j = diff[diffIdx-1]; j < numP; j++)
			{
				objectCloud[diffIdx-1][0] = pointCloud[j*3+0];
				objectCloud[diffIdx-1][1] = pointCloud[j*3+1];
				objectCloud[diffIdx-1][2] = pointCloud[j*3+2];
			}
			objectCloud[diffIdx-1][0] /= (diff[0] + numP - diff[diffIdx-1]);
			objectCloud[diffIdx-1][1] /= (diff[0] + numP - diff[diffIdx-1]);
			objectCloud[diffIdx-1][2] /= (diff[0] + numP - diff[diffIdx-1]);

			//printf("%d\n",(diff[0] + numP - diff[diffIdx-1]));
			
			/*
			printf("\n");
			for(int i=0; i<diffIdx; i++)
			{
				printf("%f %f %f\n", objectCloud[i][0], objectCloud[i][1], objectCloud[i][2]);
			}
			printf("\n");
			*/
			memcpy(preObjectCloud, objectCloud, sizeof(double)*10*3);
			

			//v_CAD
			//Dynamic object 1
			double P_roD[3] = {0, 0, 0};
			double n_roD[2] = {0, 0 };
			double thetaD, R_oD, R_iD, norm_ProD;
			double P_com;
			double mu_barD1, mu_barD2;
			double v_roD;
			double v_rel[3] = {0, 0, 0};
			double rho, xi;
			double norm_vrel;
			P_roD[0] = dObjectPos[0][0] - dronePos[0];
			P_roD[1] = dObjectPos[0][1] - dronePos[1];
			P_roD[2] = dObjectPos[0][2] - dronePos[2];
			norm_ProD = sqrt(pow(P_roD[0],2) + pow(P_roD[1],2));
			n_roD[0] = P_roD[0] / norm_ProD;
			n_roD[1] = P_roD[1] / norm_ProD;

			v_rel[0] = droneVel[0] - dObjectVel[0][0];
			v_rel[1] = droneVel[1] - dObjectVel[0][1];
			v_rel[2] = droneVel[2] - dObjectVel[0][2];
			norm_vrel = sqrt(pow(v_rel[0], 2) + pow(v_rel[1], 2));				
			v_roD = (v_rel[0] * n_roD[0]) + (v_rel[1] * n_roD[1]);
			d_min=v_roD*v_roD / (2*a_max);

			thetaD = 0;
			//thetaD = atan2(P_roD[0], P_roD[1]);
			//thetaD = thetaD + droneOri[2] - PI/2;
			
			R_oD = (a_fo * b_fo) / pow(pow(fabs(a_fo*sin(thetaD)),n) + pow(fabs(b_fo*cos(thetaD)),n), 1.0/n);
			R_iD = (a_fi * b_fi) / pow(pow(fabs(a_fi*sin(thetaD)),n) + pow(fabs(b_fi*cos(thetaD)),n), 1.0/n);

			if(norm_ProD < R_oD)
			{
				//xi =  (pow(fabs(a_fo*sin(theta)),n-1) * )/ pow(pow((pow(fabs(a_fo*sin(theta)),n) + pow(fabs(b_fo*cos(theta)),n)), 1+n), 1.0/n);
				//temp value
				mu_barD1 = norm_in;
				mu_barD2 = norm_vrel;

				if(norm_ProD < R_iD)
				{
					v_caD[0][0] = (- mu_barD1 * n_roD[0]) + mu_barD2 * (v_rel[0] + v_roD * n_roD[0]) / norm_ProD;
					v_caD[0][1] = (- mu_barD1 * n_roD[1]) + mu_barD2 * (v_rel[1] + v_roD * n_roD[1]) / norm_ProD;
				}
				else
				{
					rho = (norm_ProD - R_iD) / (R_oD - R_iD) - 1;
					v_caD[0][0] = (mu_barD1 * rho * n_roD[0]) - mu_barD2 * rho * (v_rel[0] + v_roD * n_roD[0]) / norm_ProD;
					v_caD[0][1] = (mu_barD1 * rho * n_roD[1]) - mu_barD2 * rho * (v_rel[1] + v_roD * n_roD[1]) / norm_ProD;
				}

			}
			else
			{
				v_caD[0][0] = 0;
				v_caD[0][1] = 0;
			}


			//Dynamic object 2
			P_roD[0] = dObjectPos[1][0] - dronePos[0];
			P_roD[1] = dObjectPos[1][1] - dronePos[1];
			P_roD[2] = dObjectPos[1][2] - dronePos[2];
			norm_ProD = sqrt(pow(P_roD[0],2) + pow(P_roD[1],2));
			n_roD[0] = P_roD[0] / norm_ProD;
			n_roD[1] = P_roD[1] / norm_ProD;

			v_rel[0] = droneVel[0] - dObjectVel[1][0];
			v_rel[1] = droneVel[1] - dObjectVel[1][1];
			v_rel[2] = droneVel[2] - dObjectVel[1][2];
			norm_vrel = sqrt(pow(v_rel[0], 2) + pow(v_rel[1], 2));				
			v_roD = (v_rel[0] * n_roD[0]) + (v_rel[1] * n_roD[1]);
			d_min=v_roD*v_roD / (2*a_max);
			thetaD = 0;
			//thetaD = atan2(P_roD[0], P_roD[1]);
			//thetaD = thetaD + droneOri[2] - PI/2;
			
			R_oD = (a_fo * b_fo) / pow(pow(fabs(a_fo*sin(thetaD)),n) + pow(fabs(b_fo*cos(thetaD)),n), 1.0/n);
			R_iD = (a_fi * b_fi) / pow(pow(fabs(a_fi*sin(thetaD)),n) + pow(fabs(b_fi*cos(thetaD)),n), 1.0/n);

			if(norm_ProD < R_oD)
			{
				//xi =  (pow(fabs(a_fo*sin(theta)),n-1) * )/ pow(pow((pow(fabs(a_fo*sin(theta)),n) + pow(fabs(b_fo*cos(theta)),n)), 1+n), 1.0/n);
				//temp value
				mu_barD1 = norm_in;
				mu_barD2 = norm_vrel;
				if(norm_ProD < R_iD)
				{
					v_caD[1][0] = (- mu_barD1 * n_roD[0]) + mu_barD2 * (v_rel[0] + v_roD * n_roD[0]) / norm_ProD;
					v_caD[1][1] = (- mu_barD1 * n_roD[1]) + mu_barD2 * (v_rel[1] + v_roD * n_roD[1]) / norm_ProD;
				}
				else
				{
					rho = (norm_ProD - R_iD) / (R_oD - R_iD) - 1;
					v_caD[1][0] = (mu_barD1 * rho * n_roD[0]) - mu_barD2 * rho * (v_rel[0] + v_roD * n_roD[0]) / norm_ProD;
					v_caD[1][1] = (mu_barD1 * rho * n_roD[1]) - mu_barD2 * rho * (v_rel[1] + v_roD * n_roD[1]) / norm_ProD;
				}

			}
			else
			{
				v_caD[1][0] = 0;
				v_caD[1][1] = 0;
			}

			v_caD[0][0] = -v_caD[0][0];
			v_caD[0][1] = -v_caD[0][1];
			v_caD[1][0] = -v_caD[1][0];
			v_caD[1][1] = -v_caD[1][1];

			v_ref[0] = v_ref[0] + v_caD[0][0];
			v_ref[1] = v_ref[1] + v_caD[0][1];
			v_ref[0] = v_ref[0] + v_caD[1][0];
			v_ref[1] = v_ref[1] + v_caD[1][1];

			//Transform global coordinate to local coordinate
			v_ref_cv[0] = v_ref[1] * sin(droneOri[2]) + v_ref[0] * cos(droneOri[2]);
			v_ref_cv[1] = v_ref[1] * cos(droneOri[2]) - v_ref[0] * sin(droneOri[2]);

			cmdMsg.linear.x = v_ref_cv[0];
			cmdMsg.linear.y = v_ref_cv[1];
			cmdMsg.linear.z = v_in[2];
			
			//test command
			//cmdCMsg.linear.x = v_in[0];
			//cmdCMsg.linear.y = v_in[1];
			//cmdCMsg.linear.z = v_in[2];
			cmdMsg.angular.z = setDroneAngular;
		}
		else
		{
			cmdMsg.linear.x = 0;
			cmdMsg.linear.y = 0;
			cmdMsg.linear.z = 0;
			cmdMsg.angular.z = 0;
		}
		//printf("%lf / %lf %lf / %lf %lf / %lf %lf / %lf %lf\n", norm_in2, v_in[0], v_in[1], v_ref[0], v_ref[1], v_ca[0], v_ca[1], v_ca2[0], v_ca2[1]);
		//printf("%lf / %lf %lf / %lf %lf / %lf %lf\n", norm_in2, v_ref[0], v_ref[1], v_caD[0][0], v_caD[0][1], v_caD[1][0], v_caD[1][1]);
		cmdPub.publish(cmdMsg);

	};

private:
	ros::NodeHandle nh;
	//std::string takeOffPubName;
	//std::string landPubName;
	//std::string resetPubName;
	std::string cmdPubName;
	//std::string cmdPubCName;
	//std::string optiR1SubName;
	//std::string bebopconSubName;
	std::string vrepDronePoseName;
	std::string vrepDroneVelName;
	std::string vrepDObject1PoseName;
	std::string vrepDObject1VelName;
	std::string vrepDObject2PoseName;
	std::string vrepDObject2VelName;
	//ros::Publisher takeOffPub;
	//ros::Publisher landPub;
	//ros::Publisher resetPub;
	ros::Publisher cmdPub;
	//ros::Publisher cmdPubC;
	//std_msgs::Empty takeOffMsg;
	//std_msgs::Empty landMsg;
	//std_msgs::Empty resetMsg;
	geometry_msgs::Twist cmdMsg;
	//geometry_msgs::Twist cmdCMsg;

	//geometry_msgs::TwistConstPtr bebopconData;
	//geometry_msgs::PoseStamped optiR1CurData;
	//geometry_msgs::PoseStamped optiR1PreData;

	//ros::Subscriber optiSub;
	//ros::Subscriber bebopSub;

	ros::Subscriber vrepDronePose;
	ros::Subscriber vrepDroneVel;
	ros::Subscriber vrepDObject1Pose;
	ros::Subscriber vrepDObject1Vel;
	ros::Subscriber vrepDObject2Pose;
	ros::Subscriber vrepDObject2Vel;

	int vrepClientID;

	//time variable
	
	time_t T1, T2, T3, T4, T5;
	int tflag;
	double gap;
	
	//init SEPF
	double pointCloud[numP3];
	double avoidList[numP3];
	double avoidList2[numP3];
	double dronePos[3];
	double droneOri[3];
	double droneVel[3];
	double droneVelNorm;
	double droneInpAng;
	double droneVelAng;
	double dObjectPos[2][3];
	double dObjectVel[2][3];
	double a_fo, b_fo, a_fi, b_fi, a_bi, d_min, d_min2;
	double v_in[3];
	double v_ref[3];
	double v_ref_cv[3];
	double setDroneVel;
	double setDroneVelGap;
	double setDroneAngular;
	double norm_in, norm_in2, a_max;
	double max_ca[3];
	double max_ca2[3];
	double min_ca[3];
	double min_ca2[3];
	double v_ca[3];
	double v_ca2[3];
	double v_caD[2][3];
	tf::Quaternion quater;
	int n;
	int i,j;
	int t_f;
	double at, ant, bt, bnt;
	double ax, ay, az, bx, by, bz;
	double div_time;

	//Calculate obj velocity
	double objectCloud[10][3];
	double preObjectCloud[10][3];
	double preDiff;
	double currentDiff;
	double preDist;
	double currentDist;
	double numObject;
	double numObjectIdx[2];
	int pointIdx;
	int diff[10];
	int diffIdx;
};



void* joyThMain(void* arg)
{
	int joy_fd;
	struct js_event js;
	int retRead = 0;
	if((joy_fd = open(JOY_DEV, O_RDONLY)) == -1)
	{
		printf("Cloud't open the joystick \n");
	}
	else{
		fcntl(joy_fd, F_SETFL, O_NONBLOCK);
		printf("Connect joy\n");
	}
	while(1)
	{
		
		retRead = read(joy_fd, &js, sizeof(struct js_event));
		switch(js.type & ~ JS_EVENT_INIT)
		{
			case JS_EVENT_AXIS:
				jsaxes[js.number] = js.value;
				break;
			case JS_EVENT_BUTTON:
				jsbuttons[js.number] = js.value;
				break;
		}

	}

};




int main(int argc, char** argv)
{
	
	//ROS init
	ros::init(argc, argv, "sepf_lidar_bebop");

	//Create joy thread
	pthread_t joyTh;
	pthread_create(&joyTh, NULL, joyThMain, NULL);
	Sepf sepf_LB;
	int t_flag = 0;
	time_t startTime=0, endTime=0;
	double gap;
	while(ros::ok())
	{
		sepf_LB.sepf2D();
		ros::spinOnce();
	}

	return 0;
}

This snippet took 0.06 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).