[ros-users] [Discourse.ros.org] [ROS Projects] Controller

cfis ros.discourse at gmail.com
Tue Feb 27 19:25:27 UTC 2018



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;	
}





---
[Visit Topic](https://discourse.ros.org/t/controller/4060/1) or reply to this email to respond.




More information about the ros-users mailing list