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