예제 #1
0
// Returns x,y velocity vector desired by user as values from -500ish to 500ish.
std::pair<int, int> cRCreceiver::getInputMovementSpeed(void)
{
	int x = PWs_in_us[RC_RHS_LEFTRIGHT_PIN] - RC_PW_OFFSET;
	int y = PWs_in_us[RC_RHS_UPDOWN_PIN] - RC_PW_OFFSET;

	std::pair<int, int> velocity_vector(x, y);

	return velocity_vector;
}
예제 #2
0
int main(int argc, char **argv)
{
     ros::init(argc, argv, "cycle_node");
     ros::NodeHandle n;
     ros::ServiceClient client = n.serviceClient<visual_servo_control::request_servo_velocity_vector>("/visual_servo_control_node/request_servo_velocity_vector");
  
     bool first = true;
     ros::Rate rate(10.0);
     while (n.ok())
     {
          std::string request_message;
          if(first)
          {
               request_message = "initialize";
               visual_servo_control::request_servo_velocity_vector srv;  
               srv.request.request_message = request_message; 
               client.call(srv);
               first = false;
          }
          else
          {
               request_message = "cycle";
               visual_servo_control::request_servo_velocity_vector srv;  
               srv.request.request_message = request_message;  
                                   
               if (client.call(srv))
               {                
                    std::vector<double> velocity_vector(srv.response.servo_velocity_vector );
		          ROS_INFO("Velocity vector has been recieved: %f %f %f %f %f %f", velocity_vector[0] , velocity_vector[1] , velocity_vector[2] , velocity_vector[3] , velocity_vector[4] , velocity_vector[5] );
               }
               else
               {
                   ROS_INFO("Could not call service.");
               }
          }
          ros::spinOnce();
          rate.sleep();
     }     
     return 0;
}
bool getVelocityVector_Point(visual_servo_control::request_servo_velocity_vector::Request &req, visual_servo_control::request_servo_velocity_vector::Response &res)
{
	// This part of the function gets locked if it is still calculating the next velocity
	// vector in the servo application. Hence, this service will return false if it is still busy. 
	ROS_INFO("Service called (getVelocityVector_Point)");
	std::string request_message = req.request_message.c_str();
	if(!program_lock)
	{
	     ROS_INFO("Program not locked, starting getVelocityVector_Point");
		program_lock = true;
          try {
               // If the servo system is not yet initialized, then initialize global variables.
               // The control message is compared to the incoming request message.
               std::string control_message = "initialize";
               if(!servo_initialized && camera_parameters_loaded && request_message == control_message)
               {       
                    ROS_INFO("Initializing servo. Camera are parameters loaded. Service request message was : %s", control_message.c_str());

                    int cog_x = req.pepper_blob_center_of_gravity_x;
                    int cog_y = req.pepper_blob_center_of_gravity_y;

                         // Check if found center of gravity is found.
                         if(cog_x > -1 && cog_y > -1)
                         {
                              // Create point from found coordinates in image.
                              vpImagePoint cog_image_coordinate = vpImagePoint (cog_x, cog_y);
            
                              // Set intrinsic camera parameters, since we will have to convert image pixel 
                              // coordinates to visual features expressed in meters.
                              // vpCameraParameters(px,py,u0,v0) where:
                              //   -px = horizontal pixel size. 
                              //   -py = vertical pixel size.
                              //   -u0 = X-Coordinate of image center.
                              //   -v0 = Y-Coordinate of image center.
                              // px and py are the ratios between the focal and the size of the pixel on the CCD
                              // so a let say a 6mm lens with a 10micrometers pixel size gi ves you px = 6e-3/1e-5 = 600
                              // In our case (F/Sx) : 0.00546213886287705 / 1.09363999535642e-05 = 500.
                              vpCameraParameters camera_params(500, 500, 240, 320);
                              //ROS_INFO("Real image center coordinates in image (x,y) : %f %f", camera_parameters[8].D(), camera_parameters[9].D() );

                              // Create a vpFeaturePoint using a vpFeaturePoint and the parameters of the camera. 
                              // The vpDot contains only the pixel coordinates of the point in an image. 
                              // Thus this method uses the camera parameters to compute the meter coordinates 
                              // in x and y in the image plane.
                              vpFeatureBuilder::create(p_cog, camera_params, cog_image_coordinate);   

                              // It is not possible to compute the depth of the point Z (meter) in the camera 
                              // frame in the vpFeatureBuilder above.  
                              // This coordinate is needed in vpFeaturePoint to compute the interaction matrix. 
                              p_cog.set_Z(0.40);  

                              // Sets the desired position (x,y,z) of the visual feature.
                              pd_cog.buildFrom(0,0,0.40);

                              // Define the task: eye-in-hand. 
                              // The robot is hence controlled in the camera frame.
                              task.setServo(vpServo::EYEINHAND_CAMERA);

                              // The result should be a point on a point.
                              task.addFeature(p_cog,pd_cog);

                              // Set the gain of the servo task: rate of change.
                              task.setLambda(0.99);

                              // Display task information.
                              task.print();

                              // The servo loop will start at iteration zero.
                              iteration = 0;
                              servo_initialized = true;
                              res.initialized = true;
                              res.found_vector = false;
                              ROS_INFO("Servo initialized.");
                         }
                         else
                         {
                              ROS_INFO("Servo not initialized : center of gravity not found in image.");                         
                              servo_initialized = false;
                              res.initialized = false;
                              res.found_vector = false;
                         }
               }  
               else
               {
                    // When initialized, perform servo loop.
                    std::string control_message = "cycle";
                    if(servo_initialized && request_message == control_message)
                    {
                         ROS_INFO("Performing servo loop iteration : %d", iteration);

                         int cog_x = req.pepper_blob_center_of_gravity_x;
                         int cog_y = req.pepper_blob_center_of_gravity_y;
                      
                              // Check if found center of gravity is found.
                              if(cog_x > -1 && cog_y > -1)
                              {
                                   // Create point from found coordinates in image.
                                   vpImagePoint cog_image_coordinate = vpImagePoint (cog_x, cog_y);
              
                                   // Set intrinsic camera parameters, since we will have to convert image pixel 
                                   // coordinates to visual features expressed in meters.
                                   // vpCameraParameters(px,py,u0,v0) where:
                                   //   -px = horizontal pixel size. 
                                   //   -py = vertical pixel size.
                                   //   -u0 = X-Coordinate of image center.
                                   //   -v0 = Y-Coordinate of image center.
                                   vpCameraParameters camera_params(500, 500, 240, 320);

                                   // Create a vpFeaturePoint using a vpFeaturePoint and the parameters of the camera. 
                                   // The vpDot contains only the pixel coordinates of the point in an image. 
                                   // Thus this method uses the camera parameters to compute the meter coordinates 
                                   // in x and y in the image plane.
                                   vpFeatureBuilder::create(p_cog, camera_params, cog_image_coordinate);   

                                   // It is not possible to compute the depth of the point Z (meter) in the camera 
                                   // frame in the vpFeatureBuilder above.  
                                   // This coordinate is needed in vpFeaturePoint to compute the interaction matrix. 
                                   p_cog.set_Z(0.40);      

                                   // Compute the visual servoing velocity vector.
                                   vpColVector v = task.computeControlLaw();

                                   // Get feature error (s-s*) for the feature point. For this feature
                                   // point, we have 2 errors (along x and y axis).
                                   // This error is expressed in meters in the camera frame
                                   vpColVector error = task.getError(); 
                                   res.error_y = error[0];
                                   res.error_x = error[1];
                                   ROS_INFO("Feature error (x,y): (%0.5f , %0.5f)", res.error_x, res.error_y);  

                                   // Fill and send velocity vector as response to this service.
                                   std::vector<double> velocity_vector(6);
                                   velocity_vector[0] = v.operator[](0); 
                                   velocity_vector[1] = v.operator[](1); 
                                   velocity_vector[2] = v.operator[](2); 
                                   velocity_vector[3] = v.operator[](3); 
                                   velocity_vector[4] = v.operator[](4); 
                                   velocity_vector[5] = v.operator[](5); 
                                   
                                   // Send velocity vecotor as a response.
                                   res.servo_velocity_vector     = velocity_vector;
                                   res.found_vector              = true;
                                   res.initialized               = true; 
		                         ROS_INFO("Velocity vector has been sent: %f %f %f %f %f %f", velocity_vector[0], velocity_vector[1], velocity_vector[2], velocity_vector[3], velocity_vector[4], velocity_vector[5]);

                                   // End this servo iteration.  
                                   iteration++;
                                   ROS_INFO("Servo iteration performed, velocity vector send.");
                              }
                              else
                              {
                                   ROS_INFO("Servo iteration failed : could not find center of gravity. Retry.");                                                       
                              
                                   // Fill and send empty velocity vector as response to this service.
                                   std::vector<double> velocity_vector(6);
                                   res.servo_velocity_vector = velocity_vector;
                                   res.found_vector = false; 
                                   res.initialized = true;
                              }
                         }
                         else
                         {
                               if(!servo_initialized)
                               {
                                   ROS_INFO("Servo function not yet initialized.");
                                   res.found_vector = false;
                                   res.initialized = false; 
                               }
                               if(request_message != control_message )
                               {
                                   ROS_INFO("Wrong request message, use: %s", control_message.c_str() );
                                   res.found_vector = false;
                                   res.initialized = false; 
                               }
                         }
               }       
          }
          catch(vpException e) 
          {
               ROS_ERROR("Could not calculate velocity vector in getVelocityVector_Point");
               res.found_vector = false;
               res.initialized = false; 
          }     
     	program_lock = false;
		return true;
     }
	else
	{
		ROS_INFO("Visual servo calculation currently already executing and service (getVelocityVector_Point) is therefore locked.");
		return false;
	}

}