void callback(const sensor_msgs::ImageConstPtr &imgPtr){
cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
diff =cv_bridge::toCvCopy(imgPtr);
cvImageFromROS->image.copyTo(image);
diff->image.copyTo(DiffImage);
out_msg.header = imgPtr->header;
out_msg.encoding = "mono16";
int cols=image.cols;
int rows=image.rows;
ushort v;
cv::Point p;
for(int i=0;i<cols;i++){
for(int j=0;j<rows;j++){
p.x=i;
p.y=j;
v=((float)image.at<ushort>(p));
v*=multiplier->cell(p.y,p.x,v);
image.at<ushort>(p)=(ushort)v;
}
}
out_msg.image = image;
pub.publish(out_msg.toImageMsg());
out_msg.image = image-DiffImage;
pub2.publish(out_msg.toImageMsg());
//std::cout<<"global diff: "<< cv::sum(out_msg.image)/(out_msg.image.rows*out_msg.image.cols)<<std::endl;
}
void faceDetect(){
//Detect faces
cv::cvtColor( frame, frame_gray, CV_BGR2GRAY );
cv::equalizeHist( frame_gray, frame_gray );
face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
//for each face draws an ellipse arround and look for the red color at a distance from
for( i = 0; i < faces.size(); i++ )
{
cv::Point center( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 );
cv::ellipse( frame, center, cv::Size( faces[i].width*0.5, faces[i].height*0.5), 0, 0, 360, cv::Scalar( 0, 255, 0 ), 2, 8, 0 );
faceX = (float) faces[i].x;
faceY = (float) faces[i].y;
if( ((faceX + faceColorThresh) > (posX ) | (faceX - faceColorThresh) < (posX )) ) {
face = true;
//publishing camera image
out_msg.image = frame; //frame
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
msg = out_msg.toImageMsg();
pub.publish(msg);
ROS_FATAL("PERSON DETECTED");
break;
}
}
}
inline void publish(Mat img, imgColorType t, ros::Time now =
ros::Time::now())
{
hasListener=(img_pub.getNumSubscribers() > 0);
if (!enable || !hasListener)
return;
Mat res;
switch (t)
{
case gray:
msg.encoding = sensor_msgs::image_encodings::MONO8;
res = img;
break;
case hsv:
msg.encoding = sensor_msgs::image_encodings::BGR8;
cvtColor(img, res, CV_HSV2BGR);
break;
case bgr:
default:
msg.encoding = sensor_msgs::image_encodings::BGR8;
res = img;
break;
}
msg.header.stamp = now;
msg.image = res;
img_pub.publish(msg.toImageMsg());
}
void callback(const sensor_msgs::ImageConstPtr &imgPtr){
cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
cvImageFromROS->image.copyTo(image);
out_msg.header = imgPtr->header;
out_msg.encoding = "mono16";
out_msg.image = image;
pub.publish(out_msg.toImageMsg());
}
/***********************************************************
* @fn main(int argc, char **argv)
* @brief starts the Pot_Nav node and publishises twist when
* it gets a new image asuming 30 hz
***********************************************************/
int main(int argc, char **argv)
{
ros::init(argc, argv, "Pot_Nav");
ros::NodeHandle n;
image_transport::ImageTransport it(n);
dynamic_reconfigure::Server<MST_Potential_Navigation::Pot_Nav_ParamsConfig> srv;
dynamic_reconfigure::Server<MST_Potential_Navigation::Pot_Nav_ParamsConfig>::CallbackType f;
f = boost::bind(&setparamsCallback, _1, _2);
srv.setCallback(f);
//create subsctiptions
image_sub_edges = it.subscribe( n.resolveName("image_edges") , 1, edgesCallback );
//create publishers
twist_pub = n.advertise<geometry_msgs::Twist>( "nav_twist" , 5 );
map_pub = it.advertise( "map" , 5 );
//set rate to 30 hz
ros::Rate loop_rate(30);
//run main loop
while (ros::ok())
{
//check calbacks
ros::spinOnce();
//finds and publishes new twist
if(map_changed)
{
geometry_msgs::Twist twist;
twist = find_twist();
//publish twist
twist_pub.publish(twist);
//publish map
map_pub.publish(map_dis.toImageMsg());
map_changed = 0;
}
loop_rate.sleep();
}
return 0;
}
void HiwrCameraControllerNodelet::publishFrame(Mat frame ) {
out_msg_.encoding = sensor_msgs::image_encodings::MONO8;
out_msg_.image = frame;
out_msg_.header.seq = published_frame_;
char frame_id[20];
sprintf(frame_id , "%d" , published_frame_);
out_msg_.header.frame_id = frame_id;
out_msg_.header.stamp = ros::Time::now();
published_frame_++;
image_publisher_.publish(out_msg_.toImageMsg() );
}
void mapSubCallback(const nav_msgs::OccupancyGrid map) {
//================================================================MAP TO IMAGE
MAP_X = map.info.width;
MAP_Y = map.info.height;
MAP_RESOLUTION = map.info.resolution;
if ((MAP_X < 3) || (MAP_Y < 3) ) {
ROS_INFO("Map size is only x: %ld, y: %ld . Not running map to image conversion", MAP_X, MAP_Y);
return;
}
cv::Mat *map_mat = &cv_img.image;
// resize cv image if it doesn't have the same dimensions as the map
if ((map_mat->rows != MAP_Y) && (map_mat->cols != MAP_X)) {
*map_mat = cv::Mat(MAP_Y, MAP_X, CV_8U);
}
// ROS_INFO("Fault_1");
const std::vector<int8_t>&map_data(map.data);
unsigned char *map_mat_data_p = (unsigned char *)map_mat->data;
// ROS_INFO("Fault_2");
//We have to flip around the y axis, y for image starts at the top and y for map at the bottom
int MAP_Y_rev = MAP_Y - 1;
// ROS_INFO_STREAM("Fault for :" << MAP_Y_rev);
for (int y = MAP_Y_rev; y >= 0; --y) {
int idx_map_y = MAP_X * (MAP_Y - y);
int idx_img_y = MAP_X * y;
// ROS_INFO_STREAM("Fault_3 :" << y);
// if (y == 0) {
// ROS_INFO_STREAM("MAP_X :" << MAP_X);
// }
for (int x = 0; x < MAP_X; ++x) {
int idx = idx_img_y + x;
//
// if (y == 0 && x > 1000) {
// ROS_INFO_STREAM("x :" << x);
// try {
// //ROS_INFO_STREAM("valid:" << isValidPtr(map_data, idx_map_y + x));
// ROS_INFO_STREAM("Map data :" << map_data[idx_map_y + x]);
// } catch (std::exception& e) {
// std::cout << e.what() << std::endl;
// }
// }
switch (map_data[idx_map_y + x]) {
case -1:
map_mat_data_p[idx] = 255; //grey color:unknown default value of 127
break;
case 0:
map_mat_data_p[idx] = 255; // laser ray:default 255
break;
case 100:
map_mat_data_p[idx] = 0; // obstacle:default 0
break;
}
// if (y == 0 && x > 1000) {
// ROS_INFO_STREAM("x :" << x);
// }
}
// ROS_INFO_STREAM("Fault_3 :" << y);
}
// ROS_INFO("Fault_4");
//======================================================================== DETECT TREES
cv::Mat im, im_with_keypoints;
// Reduce the noise
if (GaussianBlur_kernelSize > 1) {
cv::GaussianBlur(cv_img.image, im, cv::Size(GaussianBlur_kernelSize, GaussianBlur_kernelSize), 0);
} else {
im = cv_img.image;
}
// Setup SimpleBlobDetector parameters.
cv::SimpleBlobDetector::Params params;
// Change thresholds
params.minThreshold = minThreshold;
params.maxThreshold = maxThreshold;
// Filter by Area.
if (minArea < maxArea) {
params.filterByArea = true;
params.minArea = minArea;
params.maxArea = maxArea;
} else {
params.filterByArea = false;
}
// Filter by Circularity
if (minCircularity < maxCircularity) {
params.filterByCircularity = true;
params.minCircularity = minCircularity;
params.maxCircularity = maxCircularity;
} else {
params.filterByCircularity = false;
}
// Filter by Convexity
if (minConvexity < maxConvexity) {
params.filterByConvexity = true;
params.minConvexity = minConvexity;
params.maxConvexity = maxConvexity;
} else {
params.filterByConvexity = false;
}
// Filter by Inertia
// params.filterByInertia = true;
// params.minInertiaRatio = 0.01;
// Storage for blobs
std::vector<cv::KeyPoint> keypoints;
// Set up detector with params
cv::Ptr<cv::SimpleBlobDetector> detector = cv::SimpleBlobDetector::create(params);
// Detect blobs
detector->detect(im, keypoints);
//================================================================ Publish Markers
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time();
marker.ns = "agri_mapper";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = -(MAP_X * MAP_RESOLUTION / 2) - 0.2;
marker.pose.position.y = -(MAP_Y * MAP_RESOLUTION / 2) - 0.2;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 1;
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.scale.z = 0.1;
marker.color.a = 1.0; // Don't forget to set the alpha!
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.points.resize(keypoints.size());
std::vector<cv::KeyPoint>::iterator it = keypoints.begin();
ROS_INFO_STREAM("making markers");
ROS_INFO_STREAM("\n Size : " << keypoints.size() << "\n");
for (int i = 0; it != keypoints.end(); ++it, i++) {
marker.points[i].x = (it->pt.x) * MAP_RESOLUTION;
marker.points[i].y = (MAP_Y - it->pt.y) * MAP_RESOLUTION;
marker.points[i].z = 0;
}
ROS_INFO_STREAM("made markers");
vis_pub.publish(marker);
//============================================================================PUBLISH IMAGE
// Draw detected blobs as red circles.
// DrawMatchesFlags::DRAW_RICH_KEYPOINTS flag ensures
// the size of the circle corresponds to the size of blob
cv::drawKeypoints(im, keypoints, im_with_keypoints, cv::Scalar(255, 0, 0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cv_detectionImg.image = im_with_keypoints;
image_pub.publish(cv_img.toImageMsg());
detected_pub.publish(cv_detectionImg.toImageMsg());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "people_counting");
ros::NodeHandle node;
ros::param::param<std::string>(ros::this_node::getName() + "dataset", fileDataset_param, "haarcascade_upperbody.xml");
ros::param::param<double>(ros::this_node::getName() + "scaleFactor", scaleFactor_param, 0.1);
ros::param::param<int>(ros::this_node::getName() + "minNeighbors", minNeighbors_param, 0);
// ros::param::param<double>("minSize", minSize_param, 0);
ros::param::param<double>(ros::this_node::getName() + "maxSize", maxSize_param, 1.0);
// argc--; argv++;
// while( argc && *argv[0] == '-' )
// {
// if( !strcmp(*argv, "-fps") && ( argc > 1 ) )
// {
// fps = atof(*(argv+1));
// printf("With %ffps\n",fps);
// argc--; argv++;
// }
// if( !strcmp(*argv, "-crowd_dataset") && ( argc > 1 ) )
// {
// file_dataset = *(argv+1);
// printf("Using dataset form %s\n",file_dataset.c_str());
// argc--; argv++;
// }
// argc--; argv++;
// }
std::vector<cv::Rect> objects;
std::vector<cv::Scalar> colors;
cv::RNG rng(12345);
img_msg.encoding = "bgr8";
if ( ! classifier.load(fileDataset_param))
ROS_ERROR("Can't open %s", fileDataset_param.c_str());
ros::Publisher img_pub = node.advertise<sensor_msgs::Image>("crowd/count_img", 100);
ros::Publisher count_pub = node.advertise<std_msgs::UInt16>("crowd/people_count", 100);
image_transport::ImageTransport it(node);
image_transport::Subscriber img_sub = it.subscribe("image", 1, imageCallback);
ros::Rate loop_rate(fps);
while (ros::ok())
{
if (! img_msg.image.empty()) {
classifier.detectMultiScale(img_msg.image, objects, 1.0+scaleFactor_param, minNeighbors_param+1, 0, cv::Size(), maxSize);
for (int j = 0; j < objects.size(); ++j) {
if ( j > max_detected) {
colors.push_back(cv::Scalar(rng.uniform(0,255),rng.uniform(0, 255),rng.uniform(0, 255)));
max_detected = j;
}
cv::rectangle(img_msg.image, objects[j], colors[j], 2);
}
people_count.data = (u_int16_t)objects.size();
}
img_pub.publish(img_msg.toImageMsg());
count_pub.publish(people_count);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
namespace hiwr_camera_controller
{
/** Segfault signal handler */
void sigsegvHandler(int sig)
{
signal(SIGSEGV, SIG_DFL);
fprintf(stderr, "Segmentation fault, catched by sigsegvHandler.\n");
ROS_ERROR("Segmentation fault, catched by sigsegvHandler");
ros::shutdown(); // stop the main loop
}
void HiwrCameraControllerNodelet::onInit(){
public_nh_ = getNodeHandle(); //Public namespace
private_nh_ = getMTPrivateNodeHandle();
//Retrieveing params from param/config__xx.yaml
//Can be better with have_param() before getting ...
private_nh_.getParam("absolute_exposure", config_.absolute_exposure);
private_nh_.getParam("camera_info_url", config_.camera_info_url);
private_nh_.getParam("camera_name", config_.camera_name);
private_nh_.getParam("brightness", config_.brightness);
private_nh_.getParam("contrast", config_.contrast);
private_nh_.getParam("device_name", config_.device);
private_nh_.getParam("exposure", config_.exposure);
private_nh_.getParam("focus_absolute", config_.focus_absolute);
private_nh_.getParam("focus_auto", config_.focus_auto);
private_nh_.getParam("format_mode", config_.format_mode);
private_nh_.getParam("frame_id", config_.frame_id);
private_nh_.getParam("frame_rate", config_.frame_rate);
private_nh_.getParam("gain", config_.gain);
private_nh_.getParam("height", config_.height);
private_nh_.getParam("power_line_frequency", config_.power_line_frequency);
private_nh_.getParam("saturation", config_.saturation);
private_nh_.getParam("sharpness", config_.sharpness);
private_nh_.getParam("white_balance_temperature", config_.white_balance_temperature);
private_nh_.getParam("width", config_.width);
configureSpinning(private_nh_);
it_ = new image_transport::ImageTransport(private_nh_);
image_pub_ = it_->advertise("output_video", 1);
running_ = true;
device_thread_ = boost::shared_ptr< boost::thread >
(new boost::thread(boost::bind(&HiwrCameraControllerNodelet::spin, this)));
NODELET_INFO("[UVC Cam Nodelet] Loading OK");
}
HiwrCameraControllerNodelet::HiwrCameraControllerNodelet():
private_nh_("~"),
public_nh_(config_.camera_name)
{
next_config_level_=0;
next_config_update_ = false;
next_config_count_=0;
reconfig_number_=0;
published_frame_=0;
state_ = Driver::CLOSED;
spining_state_ = true;
calibration_matches_ = true;
frame_= NULL;
}
void HiwrCameraControllerNodelet::loopGrabImage(){
std::unique_lock<std::mutex> lk(mutex_);
if(lk.owns_lock()){
lk.unlock();
}
while(ros::ok()){
int buf_idx;
if(next_config_update_){
printf("[%s] will apply nextconfig__ \n", config_.camera_name.c_str());
applyNewConfig(next_config_ , next_config_level_);
printf("[%s] Did apply nextconfig__ \n", config_.camera_name.c_str());
}
if(!spining_state_){
usleep(1000);
continue;
}
uint32_t tmp_bytes_used;
try{
buf_idx = cam_->grab(&next_frame_, &tmp_bytes_used);
}catch (std::exception e) {
printf("exception calling grab\n");
usleep(1000);
continue;
}
if (buf_idx < 0) {
printf("Could not grab image\n");
usleep(1000);
continue;
}
if(!lk.try_lock()){
cam_->release(buf_idx);
continue;
}
if(frame_!=NULL){
free(frame_);
}
frame_ = (unsigned char*)malloc(tmp_bytes_used);
memcpy(frame_, next_frame_, tmp_bytes_used);
bytes_used_ = tmp_bytes_used;
new_frame_=true;
lk.unlock();
waiter_.notify_all();
cam_->release(buf_idx);
}
}
void HiwrCameraControllerNodelet::closeCamera(){
if (state_ != Driver::CLOSED){
ROS_INFO_STREAM("[" << camera_name_ << "] closing device");
if(cam_) {
delete cam_;
}
state_ = Driver::CLOSED;
}
}
/** Open the camera device.
*
* @param newconfig_ config_uration parameters
* @return true, if successful
*
* if successful:
* state_ is Driver::OPENED
* camera_name_ set to camera_name string
*/
bool HiwrCameraControllerNodelet::openCamera(Config &new_config){
new_config.format_mode = uvc_cam::Cam::MODE_YUYV;
uvc_cam::Cam::mode_t mode = uvc_cam::Cam::MODE_YUYV;
bool success = true;
final_=NULL;
try
{
ROS_INFO("opening uvc_cam at %dx%d, %f fps - %s : %s", new_config.width, new_config.height, new_config.frame_rate , new_config.device.c_str() , findVideoDevice(new_config.device.c_str()));
printf("before cam creating, cam is at %p\n", cam_);
ROS_INFO("[Uvc Cam Nodelet] video device is {%s}", new_config.device.c_str() );
char * video_device = findVideoDevice(new_config.device.c_str());
NODELET_INFO("Video DEVICE IS %s",video_device);
cam_ = new uvc_cam::Cam(video_device, mode, new_config.width, new_config.height, new_config.frame_rate);
printf("after cam creating, cam is at %p\n", cam_);
camera_name_ = config_.camera_name ;
NODELET_INFO("[%s] camera_name_=%s",config_.camera_name.c_str(), camera_name_.c_str() );
if (camera_name_ != camera_name_)
{
camera_name_ = camera_name_;
}
state_ = Driver::OPENED;
calibration_matches_ = true;
}
catch (uvc_cam::Exception& e)
{
ROS_FATAL_STREAM("[" << camera_name_
<< "] exception opening device: " << e.what());
success = false;
}
catch(std::runtime_error& ex){
success = false;
NODELET_WARN("[UVC Cam Node] runtime_error ex : %s",ex.what());
}
return success;
}
void HiwrCameraControllerNodelet::dealMemory(){
if(final_!=NULL){
free(final_);
};
final_ = (unsigned char *) malloc( sizeof( char) * config_width_ * config_height_ );
if(final_ == NULL){
printf("############# MALLOC FAILED !! ##########\n");
}
}
bool HiwrCameraControllerNodelet::copyRead(){
std::unique_lock<std::mutex> lk(mutex_);
if(!lk.owns_lock()){
try {
lk.lock();
} catch(const std::system_error& e) {
std::cout << "coin2 Caught system_error with code " << e.code()
<< " meaning " << e.what() << '\n';
}
}
if(!new_frame_){
waiter_.wait(lk);
}
image_ipl_ = cvCreateImageHeader(cvSize(config_width_ ,config_height_), 8, 1);
dealMemory();
const int total = config_width_*config_height_*2;
if(total!=bytes_used_)
return false;
int j=0;
for(int i=0; i< total; j++){
final_[j]=frame_[i];
i+=2;
};
new_frame_=false;
lk.unlock();
return true;
}
std::unique_lock<std::mutex> HiwrCameraControllerNodelet::getLock(){
std::unique_lock<std::mutex> lk(mutex_);
if(!lk.owns_lock()){
printf(" dont get the lock \n");
try {
lk.lock();
} catch(const std::system_error& e) {
std::cout << "coin2 Caught system_error with code " << e.code()
<< " meaning " << e.what() << '\n';
}
}
printf(" do get the lock \n");
return lk;
}
/** Read camera data.
*
* @return true if successful
*/
bool HiwrCameraControllerNodelet::read() {
bool success = true;
if(config_width_ < 1 || config_height_ < 1 )
return false;
try
{
bool copy_worked = copyRead();
if(!copy_worked)
return false;
image_ipl_->imageData = (char *)final_;
}
catch (uvc_cam::Exception& e)
{
printf("exception readind data %s", e.what());
success = false;
}
return success;
}
/** Dynamic reconfig_ure callback
*
* Called immediately when callback first defined. Called again
* when dynamic reconfig_ure starts or changes a parameter value.
*
* @param newconfig_ new Config values
* @param level bit-wise OR of reconfig_uration levels for all
* changed parameters (0xffffffff on initial call)
**/
void HiwrCameraControllerNodelet::reconfig(Config &new_config, uint32_t level)
{
next_config_= new_config;
next_config_.device = config_.device;
next_config_level_ = level;
printf("################################## reconfig_ call with level %u \n" , level);
next_config_update_ = true;
next_config_count_ ++;
}
void HiwrCameraControllerNodelet::changeSize(int width, int height){
printf("############## changing size called %d %d %d \n", width, height, next_config_update_);
if(next_config_update_ == true){
printf("change sizze called, but already called before \n");
return;
}
next_config_ = Config(config_);
next_config_.width = width;
next_config_.height = height;
next_config_update_ = true;
}
void HiwrCameraControllerNodelet::applyNewConfig(Config &new_config, uint32_t level)
{
printf("### dynamic reconfig_ure level 0x%x \n", level);
ROS_DEBUG("dynamic reconfig_ure level 0x%x", level);
reconfig_number_++;
// resolve frame ID using tf_prefix parameter
if (new_config.frame_id == "")
new_config.frame_id = "camera";
ROS_DEBUG("dynamic reconfig_ure level 0x%x", level);
std::string tf_prefix = tf::getPrefixParam(private_nh_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
new_config.frame_id = tf::resolve(tf_prefix, new_config.frame_id);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (state_ == Driver::CLOSED)
{
// open with new values
if (openCamera(new_config))
{
// update camera name string
new_config.camera_name = camera_name_;
}
}
if(reconfig_number_ == 1 || config_.focus_auto != new_config.focus_auto){
try {
NODELET_INFO("will try to set autofocus to %d \n " , new_config.focus_auto);
cam_->set_control( 0x009A090C , new_config.focus_auto);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting focus_auto. Exception was " << e.what());
}
}
if(reconfig_number_ == 1 || config_.focus_absolute != new_config.focus_absolute){
try {
NODELET_INFO("will try to set focus_absolute to %d \n " , new_config.focus_absolute);
cam_->set_control( 0x009A090A , new_config.focus_absolute);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting focus_absolute. Exception was " << e.what() << "value was" << new_config.focus_absolute ) ;
}
}
config_ = new_config;
config_width_ = new_config.width;
config_height_ = new_config.height;
next_config_update_ = false;
printf("### dynamic reconfig_ure will unlock\n");
}
void HiwrCameraControllerNodelet::setSpinningState(bool p_spinning_state){
spining_state_ = p_spinning_state;
}
bool HiwrCameraControllerNodelet::getSpinningState(){
return spining_state_;
}
cv_bridge::CvImage out_msg_;
void HiwrCameraControllerNodelet::publishFrame(Mat frame ) {
out_msg_.encoding = sensor_msgs::image_encodings::MONO8;
out_msg_.image = frame;
out_msg_.header.seq = published_frame_;
char frame_id[20];
sprintf(frame_id , "%d" , published_frame_);
out_msg_.header.frame_id = frame_id;
out_msg_.header.stamp = ros::Time::now();
published_frame_++;
image_publisher_.publish(out_msg_.toImageMsg() );
}
/** driver main spin loop */\
void HiwrCameraControllerNodelet::spin() {
NODELET_INFO("[UVC Cam Nodelet] Main Loop...");
printf("inside spinOnce loop \n");
dynamic_reconfigure::Server<Config> srv;
dynamic_reconfigure::Server<Config>::CallbackType f
= boost::bind(&HiwrCameraControllerNodelet::reconfig, this, _1, _2);
srv.setCallback(f);
reconfig(config_, 0xffffffff);
applyNewConfig(config_ , 0xffffffff);
spinning_thread_ = boost::shared_ptr< boost::thread >
(new boost::thread(boost::bind(&HiwrCameraControllerNodelet::loopGrabImage, this)));
while (ros::ok())
{
if (state_ != Driver::CLOSED)
{
//Check nb subscribers
if(image_pub_.getNumSubscribers() > 0 && !next_config_update_)
{
if (spining_state_ == true && next_config_count_ > 0 && read() )
{
cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
cv_ptr->header.stamp = ros::Time::now();
cv_ptr->header.frame_id = config_.frame_id;
cv_ptr->encoding = sensor_msgs::image_encodings::MONO8; //"mono8";//"bgr8";
cv_ptr->image = Mat( image_ipl_ );
image_pub_.publish(cv_ptr->toImageMsg());
}else{
usleep(1000);//1ms sleep
}
}else{
usleep(1000);
}
}
ros::spinOnce();
}
}
bool HiwrCameraControllerNodelet::serviceSetSpinningState( hiwr_msg::SetState::Request &req ,hiwr_msg::SetState::Response &res ){
res.state = req.state;
setSpinningState(req.state);
return true;
}
bool HiwrCameraControllerNodelet::serviceGetSpinningState( hiwr_msg::GetState::Request &req ,hiwr_msg::GetState::Response &res ){
res.state =getSpinningState();
return true;
}
void HiwrCameraControllerNodelet::configureSpinning(ros::NodeHandle& nh){
NODELET_INFO("[UVCCam Nodelet] START config_uring spinning state");
ros::NodeHandle& ph = getMTPrivateNodeHandle();
service_spinning_state_setter_ = ph.advertiseService("setSpinningState", &HiwrCameraControllerNodelet::serviceSetSpinningState, this);
service_spinning_state_getter_ = ph.advertiseService("getSpinningState", &HiwrCameraControllerNodelet::serviceGetSpinningState, this);
NODELET_INFO("[UVCCam Nodelet] DONE");
}
PLUGINLIB_DECLARE_CLASS(hiwr_camera_controller,HiwrCameraControllerNodelet, hiwr_camera_controller::HiwrCameraControllerNodelet, nodelet::Nodelet);
}
void BallDetector::imageCb(const sensor_msgs::ImageConstPtr& msg){
static cv_bridge::CvImageConstPtr cv_const_ptr;
#ifdef BALLDETECTOR_DEBUG
static cv_bridge::CvImage cv_img;
cv_img.header.stamp = ros::Time::now();
if(debugLevel.sendDebugTimes){
debugTimeInit();
debugTimeStart();
}
#endif
try{
cv_const_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
}catch (cv_bridge::Exception& e){
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("cvconvert");
}
#endif
#ifdef BALLDETECTOR_DEBUG
//Get a the image just as a Mat
static cv::Mat colorImg;
if(debugLevel.sendDebugImages){
colorImg = cv_const_ptr->image.clone();
//cv_const_ptr->image.copyTo(colorImg);
}else{
colorImg = cv_const_ptr->image;
}
#else
static cv::Mat colorImg;
colorImg = cv_const_ptr->image;
#endif
//Create the hsv image
static cv::Mat hsvImg;
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("allocate");
}
#endif
//colorImg is type CV_8U
cv::cvtColor(colorImg,hsvImg,CV_RGB2HSV);
//cv::cvtColor(colorImg,hsvImg,CV_RGB2HLS);
//cv::cvtColor(colorImg,hsvImg,CV_RGB2Lab); //faster but not on gumstix?
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("hsv");
}
#endif
//Perform thresholding
static cv::Mat threshImg;
cv::inRange(hsvImg,lowThresh,highThresh,threshImg);
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("threshold");
}
#endif
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
//Send some images out
cv_img.image = threshImg;
cv_img.encoding = enc::MONO8; // TYPE_8UC1;
debugImg2_pub_.publish(cv_img.toImageMsg());
cv_img.image = hsvImg;
cv_img.encoding = enc::BGR8;
debugImg3_pub_.publish(cv_img.toImageMsg());
}
#endif /** BALLDETECTOR_DEBUG **/
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeStart();
}
#endif
//Contours
static vector<vector<cv::Point> > contours;
contours.clear();
static vector<cv::Vec4i> hierarchy;
hierarchy.clear();
//Some quick testing indicates CV_RETR_* doesn't make too much
//difference in performance. The CV_CHAIN_APPROX_* also doesn't
//seem to make much difference, but the "NONE" draws the full
//outline with debug mode (taking more time), but is easier to look
//at the output.
cv::findContours( threshImg, contours, hierarchy,
CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
//cv::findContours( threshImg, contours, hierarchy,
//CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("findcontours");
}
#endif
// iterate through all the top-level contours,
// draw each connected component with its own random color
int idxMax = -1;
unsigned int idxMaxValue = 0;
int idx = 0;
if(hierarchy.size() > 0){
for( ; idx >= 0; idx = hierarchy[idx][0] ){
//If it is large enough, fit an ellipse to it
if(contours.at(idx).size() > 15){
//Slower than the below methods
//cv::RotatedRect r = cv::fitEllipse(cv::Mat(contours.at(idx)));
//cv::Rect rect = r.boundingRect();
//Find the min/max pixel locatiosn of x and y
int maxX = 0, maxY = 0;
unsigned int maxXidx = 0, maxYidx = 0;
//Just needs to be bigger than max image width/height
int minX = 999999999, minY = 999999999;
unsigned int minXidx = 0, minYidx = 0;
for(unsigned int i = 0; i < contours.at(idx).size(); i++){
cv::Point p = contours.at(idx).at(i);
if(p.x > maxX){
maxXidx = i;
maxX = p.x;
}
else if(p.x < minX){
minXidx = i;
minX = p.x;
}
if(p.y > maxY){
maxYidx = i;
maxY = p.y;
}
else if(p.y < minY){
minYidx = i;
minY = p.y;
}
}
//Keep the ellipse if it is mostly round
//if((rect.width/(1.0*rect.height) > 0.75) && (rect.height/(1.0*rect.width) > 0.75)){
if(((1.0*(contours.at(idx).at(maxXidx).x-contours.at(idx).at(minXidx).x)/
(contours.at(idx).at(maxYidx).y-contours.at(idx).at(minYidx).y))) > 0.75
&&
((1.0*(contours.at(idx).at(maxYidx).y-contours.at(idx).at(minYidx).y)/
(contours.at(idx).at(maxXidx).x-contours.at(idx).at(minXidx).x)) > 0.75)){
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
//cv::ellipse(colorImg,r,cv::Scalar(255,0,0,0),3);
//Color all the points white (it isn't all points, but a lot on the boarders)
for(unsigned int i = 0; i < contours.at(idx).size(); i++){
cv::Point p = contours.at(idx).at(i);
//Get a pointer to the row, then access the pixel elements
colorImg.ptr<uchar>(p.y)[3*p.x] = 255;
colorImg.ptr<uchar>(p.y)[3*p.x+1] = 255;
colorImg.ptr<uchar>(p.y)[3*p.x+2] = 255;
}
//draw max/min points
/*
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(maxXidx).x - 1,
contours.at(idx).at(maxXidx).y- 1),
cv::Point(contours.at(idx).at(maxXidx).x + 1,
contours.at(idx).at(maxXidx).y+ 1),
cv::Scalar(255,0,0,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(maxYidx).x - 1,
contours.at(idx).at(maxYidx).y- 1),
cv::Point(contours.at(idx).at(maxYidx).x + 1,
contours.at(idx).at(maxYidx).y+ 1),
cv::Scalar(255,0,0,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(minXidx).x - 1,
contours.at(idx).at(minXidx).y- 1),
cv::Point(contours.at(idx).at(minXidx).x + 1,
contours.at(idx).at(minXidx).y+ 1),
cv::Scalar(255,0,0,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idx).at(minYidx).x - 1,
contours.at(idx).at(minYidx).y- 1),
cv::Point(contours.at(idx).at(minYidx).x + 1,
contours.at(idx).at(minYidx).y+ 1),
cv::Scalar(255,0,0,0),2);
*/
}
#endif /** BALLDETECTOR_DEBUG **/
//If there are more points in it than our previous max, then record it
//N.B. contours.at(idx).size() isn't all points, rather a
//representation of them...but it seems to map fairly well.
if(idxMaxValue < contours.at(idx).size()){
idxMaxValue = contours.at(idx).size();
idxMax = idx;
}
}
}
}
}
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("iteratecontours");
}
#endif
//Make sure we found a good one
if(idxMax >= 0){
//Now publish and draw the best
cv::RotatedRect ellipse = cv::fitEllipse(cv::Mat(contours.at(idxMax)));
cv::Rect rect = ellipse.boundingRect();
//Publish the message for the best
ballDetector::ballLocation circ;
circ.header.stamp = ros::Time::now();
//Use the average of the height and width as the radius
double radius = (rect.height/2.0 + rect.width/2.0)/2.0;
circ.imageWidth = colorImg.cols;
circ.imageHeight = colorImg.rows;
//Make it relative to the center of the image
circ.x = rect.x + radius - threshImg.cols/2;
circ.y = -1*(rect.y + radius - threshImg.rows/2);
circ.radius = radius;
circle_pub_.publish(circ);
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
//Draw the best one bold
cv::ellipse(colorImg,ellipse,cv::Scalar(0,255,0,0),4);
//And the center point
cv::rectangle(colorImg,
cv::Point(rect.x + radius-2,rect.y + radius-2),
cv::Point(rect.x + radius+2,rect.y + radius+2),
cv::Scalar(0,255,0,0),4);
//Find min/max x,y pixel locations to draw them
//cout << "---------------------\n";
int maxX = 0, maxY = 0;
unsigned int maxXidx = 0, maxYidx = 0;
//Just needs to be bigger than max image width/height
int minX = 999999999, minY = 999999999;
unsigned int minXidx = 0, minYidx = 0;
for(unsigned int i = 0; i < contours.at(idxMax).size(); i++){
cv::Point p = contours.at(idxMax).at(i);
//printf("%d: [%d,%d] ",i,p.x,p.y);
if(p.x > maxX){
maxXidx = i;
maxX = p.x;
}
else if(p.x < minX){
minXidx = i;
minX = p.x;
}
if(p.y > maxY){
maxYidx = i;
maxY = p.y;
}
else if(p.y < minY){
minYidx = i;
minY = p.y;
}
}
/*
printf("\nminX: %d, %d, %d, %d\n",minX,minXidx,
contours.at(idxMax).at(minXidx).x,contours.at(idxMax).at(minXidx).y);
printf("maxX: %d, %d, %d, %d\n",maxX,maxXidx,
contours.at(idxMax).at(maxXidx).x,contours.at(idxMax).at(maxXidx).y);
printf("minY: %d, %d, %d, %d\n",minY,minYidx,
contours.at(idxMax).at(minYidx).x,contours.at(idxMax).at(minYidx).y);
printf("maxY: %d, %d, %d, %d\n",maxY,maxYidx,
contours.at(idxMax).at(maxYidx).x,contours.at(idxMax).at(maxYidx).y);
printf("diffX: %d\n",contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x);
printf("diffY: %d\n",contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y);
printf("ratio: %f %f\n",
1.0*(contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x)/
(contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y),
1.0*(contours.at(idxMax).at(maxYidx).y-contours.at(idxMax).at(minYidx).y)/
(contours.at(idxMax).at(maxXidx).x-contours.at(idxMax).at(minXidx).x));
*/
//draw max/min points
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(maxXidx).x - 1,
contours.at(idxMax).at(maxXidx).y - 1),
cv::Point(contours.at(idxMax).at(maxXidx).x + 1,
contours.at(idxMax).at(maxXidx).y + 1),
cv::Scalar(0,0,255,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(maxYidx).x - 1,
contours.at(idxMax).at(maxYidx).y - 1),
cv::Point(contours.at(idxMax).at(maxYidx).x + 1,
contours.at(idxMax).at(maxYidx).y + 1),
cv::Scalar(0,0,255,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(minXidx).x - 1,
contours.at(idxMax).at(minXidx).y - 1),
cv::Point(contours.at(idxMax).at(minXidx).x + 1,
contours.at(idxMax).at(minXidx).y + 1),
cv::Scalar(0,0,255,0),2);
cv::rectangle(colorImg,
cv::Point(contours.at(idxMax).at(minYidx).x - 1,
contours.at(idxMax).at(minYidx).y - 1),
cv::Point(contours.at(idxMax).at(minYidx).x + 1,
contours.at(idxMax).at(minYidx).y + 1),
cv::Scalar(0,0,255,0),2);
}
#endif /** BALLDETECTOR_DEBUG **/
}
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugTimes){
debugTimeRecordAndReStart("publish");
}
#endif
#ifdef BALLDETECTOR_DEBUG
if(debugLevel.sendDebugImages){
cv_img.image = colorImg;
cv_img.encoding = enc::BGR8;
debugImg1_pub_.publish(cv_img.toImageMsg());
}
if(debugLevel.sendDebugTimes){
debugTimeSend();
}
#endif /** BALLDETECTOR_DEBUG **/
}
void imageDataHandler(const sensor_msgs::Image::ConstPtr& imageData)
{
timeLast = timeCur;
timeCur = imageData->header.stamp.toSec() - 0.1163;
cv_bridge::CvImageConstPtr imageDataCv = cv_bridge::toCvShare(imageData, "mono8");
if (!systemInited) {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
systemInited = true;
return;
}
Mat imageLast, imageCur;
oclMat oclImageLast, oclImageCur;
if (isOddFrame) {
remap(imageDataCv->image, image1, mapx, mapy, CV_INTER_LINEAR);
oclImage1 = oclMat(image1);
imageLast = image0;
imageCur = image1;
oclImageLast = oclImage0;
oclImageCur = oclImage1;
} else {
remap(imageDataCv->image, image0, mapx, mapy, CV_INTER_LINEAR);
oclImage0 = oclMat(image0);
imageLast = image1;
imageCur = image0;
oclImageLast = oclImage1;
oclImageCur = oclImage0;
}
isOddFrame = !isOddFrame;
resize(oclImageLast, oclImageShow, showSize);
oclImageShow.download(imageShow);
cornerHarris(oclImageShow, oclHarrisLast, 2, 3, 0.04);
oclHarrisLast.download(harrisLast);
vector<Point2f> *featuresTemp = featuresLast;
featuresLast = featuresCur;
featuresCur = featuresTemp;
pcl::PointCloud<ImagePoint>::Ptr imagePointsTemp = imagePointsLast;
imagePointsLast = imagePointsCur;
imagePointsCur = imagePointsTemp;
imagePointsCur->clear();
int recordFeatureNum = totalFeatureNum;
detectionCount = (detectionCount + 1) % (detectionSkipNum + 1);
if (detectionCount == detectionSkipNum) {
oclMat oclFeaturesSub;
for (int i = 0; i < ySubregionNum; i++) {
for (int j = 0; j < xSubregionNum; j++) {
int ind = xSubregionNum * i + j;
int numToFind = maxFeatureNumPerSubregion - subregionFeatureNum[ind];
if (numToFind > maxFeatureNumPerSubregion / 5.0) {
int subregionLeft = xBoundary + (int)(subregionWidth * j);
int subregionTop = yBoundary + (int)(subregionHeight * i);
Rect subregion = Rect(subregionLeft, subregionTop, (int)subregionWidth, (int)subregionHeight);
oclFeatureDetector.maxCorners = numToFind;
oclFeatureDetector(oclImageLast(subregion), oclFeaturesSub);
if (oclFeaturesSub.cols > 0) {
oclFeatureDetector.downloadPoints(oclFeaturesSub, featuresSub);
numToFind = featuresSub.size();
} else {
numToFind = 0;
}
int numFound = 0;
for(int k = 0; k < numToFind; k++) {
featuresSub[k].x += subregionLeft;
featuresSub[k].y += subregionTop;
int xInd = (featuresSub[k].x + 0.5) / showDSRate;
int yInd = (featuresSub[k].y + 0.5) / showDSRate;
if (harrisLast.at<float>(yInd, xInd) > 1e-7) {
featuresLast->push_back(featuresSub[k]);
featuresInd.push_back(featuresIndFromStart);
numFound++;
featuresIndFromStart++;
}
}
totalFeatureNum += numFound;
subregionFeatureNum[ind] += numFound;
}
}
}
}
if (totalFeatureNum > 0) {
Mat featuresLastMat(1, totalFeatureNum, CV_32FC2, (void*)&(*featuresLast)[0]);
oclMat oclFeaturesLast(featuresLastMat);
oclMat oclFeaturesCur, oclFeaturesStatus;
oclOpticalFlow.sparse(oclImageLast, oclImageCur, oclFeaturesLast, oclFeaturesCur, oclFeaturesStatus);
if (oclFeaturesCur.cols > 0 && oclFeaturesCur.cols == oclFeaturesStatus.cols) {
download(oclFeaturesCur, *featuresCur);
download(oclFeaturesStatus, featuresStatus);
totalFeatureNum = featuresCur->size();
} else {
totalFeatureNum = 0;
}
}
for (int i = 0; i < totalSubregionNum; i++) {
subregionFeatureNum[i] = 0;
}
ImagePoint point;
int featureCount = 0;
for (int i = 0; i < totalFeatureNum; i++) {
double trackDis = sqrt(((*featuresLast)[i].x - (*featuresCur)[i].x)
* ((*featuresLast)[i].x - (*featuresCur)[i].x)
+ ((*featuresLast)[i].y - (*featuresCur)[i].y)
* ((*featuresLast)[i].y - (*featuresCur)[i].y));
if (!(trackDis > maxTrackDis || (*featuresCur)[i].x < xBoundary ||
(*featuresCur)[i].x > imageWidth - xBoundary || (*featuresCur)[i].y < yBoundary ||
(*featuresCur)[i].y > imageHeight - yBoundary) && featuresStatus[i]) {
int xInd = (int)(((*featuresLast)[i].x - xBoundary) / subregionWidth);
int yInd = (int)(((*featuresLast)[i].y - yBoundary) / subregionHeight);
int ind = xSubregionNum * yInd + xInd;
if (subregionFeatureNum[ind] < maxFeatureNumPerSubregion) {
(*featuresCur)[featureCount] = (*featuresCur)[i];
(*featuresLast)[featureCount] = (*featuresLast)[i];
featuresInd[featureCount] = featuresInd[i];
point.u = -((*featuresCur)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresCur)[featureCount].y - kImage[5]) / kImage[4];
point.ind = featuresInd[featureCount];
imagePointsCur->push_back(point);
if (i >= recordFeatureNum) {
point.u = -((*featuresLast)[featureCount].x - kImage[2]) / kImage[0];
point.v = -((*featuresLast)[featureCount].y - kImage[5]) / kImage[4];
imagePointsLast->push_back(point);
}
featureCount++;
subregionFeatureNum[ind]++;
}
}
}
totalFeatureNum = featureCount;
featuresCur->resize(totalFeatureNum);
featuresLast->resize(totalFeatureNum);
featuresInd.resize(totalFeatureNum);
sensor_msgs::PointCloud2 imagePointsLast2;
pcl::toROSMsg(*imagePointsLast, imagePointsLast2);
imagePointsLast2.header.stamp = ros::Time().fromSec(timeLast);
imagePointsLastPubPointer->publish(imagePointsLast2);
showCount = (showCount + 1) % (showSkipNum + 1);
if (showCount == showSkipNum) {
bridge.image = imageShow;
bridge.encoding = "mono8";
sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
imageShowPubPointer->publish(imageShowPointer);
}
}
