bool RobotisOPBallTrackingNode::detectCircles(const sensor_msgs::Image& msg, cv::Point& offset)
{
cv_bridge::CvImagePtr image_trans, image_blob;
image_trans = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::RGB8);
image_blob = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::RGB8);
cv::Mat& mat_blob = image_blob->image;
cv::Mat& mat_trans = image_trans->image;
cv::GaussianBlur( mat_trans, mat_trans, cv::Size(9, 9), 2, 2 );
std::vector<cv::Mat> canales;
cv::split(mat_trans, canales);
canales[0] = 2*canales[0]-canales[1]-canales[2];
canales[1] = canales[2]-canales[2];
canales[2] = canales[2]-canales[2];
cv::merge(canales, mat_trans);
cv::cvtColor(mat_trans,mat_trans, CV_RGB2GRAY);
std::vector<cv::Vec3f> circles;
//ROS_INFO("%f %f %f %f %f %f",dp_, minDist_, param1_, param2_,min_radius_, max_radius_);
cv::HoughCircles(mat_trans, circles, CV_HOUGH_GRADIENT, dp_, minDist_, param1_, param2_,min_radius_, max_radius_);//1, mat_trans.rows/8, 200, 10, 0, 0 );
ROS_INFO("detected %i circles",(int)circles.size());
cv::Point ball_position;
bool ball_detected = false;
for( size_t i = 0; i < circles.size(); i++ )
{
ball_detected = true;
cv::Point center((circles[i][0]), (circles[i][1]));
ball_position = center;
cv::Point image_center(mat_blob.size().width/2.0,mat_blob.size().height/2.0);
offset = ball_position - image_center ;
int radius = (circles[i][2]);
// circle center
cv::circle( mat_blob, center, 3, cv::Scalar(0,255,0), -1, 8, 0 );
// circle outline
cv::circle( mat_blob, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
ball_position = center;
}
sensor_msgs::ImagePtr msg_out_trans;
image_trans->encoding = "mono8";
msg_out_trans = image_trans->toImageMsg();
msg_out_trans->header.stamp = ros::Time::now();
transformed_img_pub_.publish(*msg_out_trans);
sensor_msgs::ImagePtr msg_out_blob;
msg_out_blob = image_blob->toImageMsg();
msg_out_blob->header.stamp = ros::Time::now();
blob_img_pub_.publish(*msg_out_blob);
return ball_detected;
}
void set_image_and_center(BitmapLayer* layer, const prezr_bitmap_t* resource, GPoint location) {
bitmap_layer_set_bitmap(layer, resource->bitmap);
image_center(bitmap_layer_get_layer(layer), location, GSize(resource->width, resource->height));
}
//http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
void RobotisOPBallTrackingNode::imageCb(const sensor_msgs::Image& msg)
{
ROS_INFO("cb");
//DETECT BALL
cv_bridge::CvImagePtr image_trans, image_blob;
image_trans = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::RGB8);
image_blob = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::RGB8);
cv::Mat& mat_blob = image_blob->image;
cv::Mat& mat_trans = image_trans->image;
cv::GaussianBlur( mat_trans, mat_trans, cv::Size(9, 9), 2, 2 );
std::vector<cv::Mat> canales;
cv::split(mat_trans, canales);
canales[0] = 2*canales[0]-canales[1]-canales[2];
canales[1] = canales[2]-canales[2];
canales[2] = canales[2]-canales[2];
cv::merge(canales, mat_trans);
cv::cvtColor(mat_trans,mat_trans, CV_RGB2GRAY);
std::vector<cv::Vec3f> circles;
ROS_INFO("%f %f %f %f %f %f",dp_, minDist_, param1_, param2_,min_radius_, max_radius_);
cv::HoughCircles(mat_trans, circles, CV_HOUGH_GRADIENT, dp_, minDist_, param1_, param2_,min_radius_, max_radius_);//1, mat_trans.rows/8, 200, 10, 0, 0 );
ROS_INFO("detected %i circles",(int)circles.size());
cv::Point ball_position;
bool ball_detected = false;
float min_dist;
if(circles.size() > 0)
min_dist = squaredDistXY(previous_position_, circles[0]);
int min_idx = 0;
for( size_t i = 0; i < circles.size(); i++ )
{
ball_detected = true;
cv::Point center((circles[i][0]), (circles[i][1]));
ball_position = center;
int radius = (circles[i][2]);
// circle center
cv::circle( mat_blob, center, 3, cv::Scalar(0,255,0), -1, 8, 0 );
// circle outline
cv::circle( mat_blob, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
if(squaredDistXY(previous_position_, circles[0]) < min_dist)
{
min_idx = i;
min_dist = squaredDistXY(previous_position_, circles[0]);
ball_position = center;
}
}
if(circles.size() > 0)
{
previous_position_ = circles[min_idx];
}
sensor_msgs::ImagePtr msg_out_trans;
image_trans->encoding = "mono8";
msg_out_trans = image_trans->toImageMsg();
msg_out_trans->header.stamp = ros::Time::now();
transformed_img_pub_.publish(*msg_out_trans);
sensor_msgs::ImagePtr msg_out_blob;
msg_out_blob = image_blob->toImageMsg();
msg_out_blob->header.stamp = ros::Time::now();
blob_img_pub_.publish(*msg_out_blob);
if(ball_detected)
{
count_no_detection_ = 0;
cv::Point image_center(mat_blob.size().width/2.0,mat_blob.size().height/2.0);
cv::Point offset = ball_position - image_center ;
ROS_INFO("ball pos %i %i",ball_position.x, ball_position.y);
ROS_INFO("ball pos offset %i %i",offset.x, offset.y);
//head movement
double tilt_scale_ = 0.001;
double pan_scale_ = 0.001;
std_msgs::Float64 angle_msg;
angle_msg.data = tilt_-tilt_scale_*offset.y;
tilt_pub_.publish(angle_msg);
angle_msg.data = pan_-pan_scale_*offset.x;
pan_pub_.publish(angle_msg);
//walking
double a_scale_ = 0.5;
geometry_msgs::Twist vel;
vel.angular.z = a_scale_*(angle_msg.data);
vel.linear.x = std::max(0.0,0.8- std::abs(5.0*vel.angular.z));//l_scale_*joy->axes[axis_linear_x_];
vel.linear.y = 0;//l_scale_*joy->axes[axis_linear_y_];
vel_pub_.publish(vel);
}
else if (count_no_detection_< 10)
{
count_no_detection_++;
count_search_loop_ = 0;
}
else
{
//search loop
std_msgs::Float64 angle_msg;
angle_msg.data = sin(count_search_loop_*6.24/90);
tilt_pub_.publish(angle_msg);
geometry_msgs::Twist vel;
vel.angular.z = 0;
vel.linear.x = 0;
vel.linear.y = 0;
vel_pub_.publish(vel);
count_search_loop_++;
}
}
ossimDpt ossimH5GridModel::extrapolate( const ossimGpt& gpt ) const
{
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<< "DEBUG ossimH5GridModel::extrapolate: entering... " << std::endl;
}
theExtrapolateGroundFlag = true;
double height = 0.0;
//---
// If ground point supplied has NaN components, return now with a NaN point.
//---
if ( (ossim::isnan(gpt.lat)) || (ossim::isnan(gpt.lon)) )
{
theExtrapolateGroundFlag = false;
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<< "DEBUG ossimH5GridModel::extrapolate: returning..." << std::endl;
}
return ossimDpt(ossim::nan(), ossim::nan());
}
if(ossim::isnan(gpt.hgt) == false)
{
height = gpt.hgt;
}
if(theSeedFunction.valid())
{
ossimDpt ipt;
theSeedFunction->worldToLineSample(gpt, ipt);
theExtrapolateGroundFlag = false;
return ipt;
}
//---
// Determine which edge is intersected by the radial, and establish
// intersection:
//---
ossimDpt edgePt (gpt);
ossimDpt image_center (theRefGndPt);
if ( m_crossesDateline )
{
// Longitude points between 0 and 360.
if (edgePt.lon < 0.0 ) edgePt.lon += 360.0;
if ( image_center.lon < 0.0 ) image_center.lon += 360.0;
}
m_boundGndPolygon.clipLineSegment(image_center, edgePt);
//---
// Compute an epsilon perturbation in the direction away from edgePt for
// later computing directional derivative:
//---
const double DEG_PER_MTR = 8.983152841e-06; // Equator WGS-84...
double epsilon = theMeanGSD*DEG_PER_MTR; //degrees (latitude) per pixel
ossimDpt deltaPt (edgePt-image_center);
ossimDpt epsilonPt (deltaPt*epsilon/deltaPt.length());
edgePt -= epsilonPt;
ossimDpt edgePt_prime (edgePt - epsilonPt);
//---
// Establish image point corresponding to edge point and edgePt+epsilon:
//---
ossimGpt edgeGP (edgePt.lat, edgePt.lon, height);//gpt.hgt);
ossimGpt edgeGP_prime (edgePt_prime.lat, edgePt_prime.lon, height);//gpt.hgt);
if ( m_crossesDateline )
{
// Put longitude back between -180 and 180.
edgeGP.limitLonTo180();
edgeGP_prime.limitLonTo180();
}
worldToLineSample(edgeGP, edgePt);
worldToLineSample(edgeGP_prime, edgePt_prime);
//---
// Compute approximate directional derivatives of line and sample along
// radial at the edge:
//---
double dsamp_drad = (edgePt.samp - edgePt_prime.samp)/epsilon;
double dline_drad = (edgePt.line - edgePt_prime.line)/epsilon;
//---
// Now extrapolate to point of interest:
//---
double delta = (ossimDpt(gpt) - ossimDpt(edgeGP)).length();
ossimDpt extrapolated_ip (edgePt.samp + delta*dsamp_drad,
edgePt.line + delta*dline_drad);
if (traceDebug())
{
ossimNotify(ossimNotifyLevel_DEBUG)
<< "DEBUG ossimH5GridModel::extrapolate: returning..." << std::endl;
}
theExtrapolateGroundFlag = false;
return extrapolated_ip;
} // ossimH5GridModel::extrapolate
