Controller

ROS Projects
1

Good evening dear community of ROS, I am a Cuban boy of 12 years who by chance discovered the simulator gazebo ros, but due to the poor communication in my country I have limited access to the Internet. Currently I set my goal to develop the simulation of a quadrator in ros, which is able to locate a mobile platform, and land on it, but I have stumbled on the first problem to achieve it, I do not know how to implement a controller that allows me to rotate the quadrator on the z-axis, that is, its yaw, from its rotation matrix.

Best regards, God bless you.

PS: I attached a part of the code, in which you can see the way I currently perform the rotation without using the rotation matrix.

//-------------- YAW CONTROL ----------------
max_grad = 180.0;
yaw_error = yaw_reference - yaw;
u = kp_yaw*yaw_error/max_grad;

		commandPilot.angular.z = u;
		

		//------------ ALTITUDE CONTROL ---.---------

		altura_error = alt_ref - altitude;
		commandPilot.linear.z = kp_alt * altura_error;
			

		if(commandPilot.linear.z > 0.5)
			commandPilot.linear.z = 0.5;
	
		if(fabsf(altura_error) < 0.01 && fabsf(yaw_error) < 0.5)
		{
			state = 1;
			commandPilot.linear.x = 0;
			commandPilot.linear.y = 0;
			pose_xi = start_pose.position.x;
			pose_yi = start_pose.position.y;
			//~ ROS_INFO_STREAM("Gazebo pose: X = " << pose_xi << " Y = " << pose_yi);
		}				
		
	}else
	if(state == 1)
	{	
		putText(front_image," Dist: "+ stDist.str() + " Dist error: "+ stErrorDist.str() + " Signal: " + stSignal.str(), Point( 5, 55), FONT_HERSHEY_COMPLEX_SMALL, 0.7, CV_RGB( 255, 255, 255), 1, 1);
	
		dist_x = distancia - fabsf(pose_xi);
		dist_error = dist_x - start_pose.position.x;
		signal = kp * dist_error;
		
		commandPilot.linear.x = signal;
		
		if(fabsf(dist_x) < 0.01)
		{
			commandPilot.linear.x = 0;
			commandPilot.linear.y = 0;				
		}			
	}

void image_viewer::Ardrone_odom(const sensor_msgs::Imu::ConstPtr &odom)
{
odom_ARdrone_x = odom->orientation.x;
odom_ARdrone_y = odom->orientation.y;
odom_ARdrone_z = odom->orientation.z;
odom_ARdrone_w = odom->orientation.w;

//my code

/*x0 = odom_ARdrone_x;
y0 = odom_ARdrone_y;
z0 = odom_ARdrone_z;
w0 = odom_ARdrone_w;*/


tf::Quaternion q1(odom_ARdrone_x,odom_ARdrone_y,odom_ARdrone_z,odom_ARdrone_w);
tf::Matrix3x3 m(q1);

//Found final position

//z1 = r31x0 + r32y0 + r33z0


m.getRPY(roll, pitch, yaw);

roll = (roll * 180) / M_PI;
pitch = (pitch * 180) / M_PI;
yaw = (yaw * 180) / M_PI;

}

2

Hello mate,

Not trying to self promote here, but this extremely relevant ROS tutorial was written some weeks ago. Check it out! (Oh, and keep in mind that you should use ROS answers for such questions instead of discourse!)

Have fun,

George

1 Like