void targetSizeCb( int val, void *dummy )
{
  std_msgs::Float32 msg;
  msg.data = val;
  targetSizePub.publish(msg);
}
void hRangeCb( int val, void *dummy )
{
  std_msgs::Int32 msg;
  msg.data = val;
  hRangePub.publish( msg );
}
  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    // Now, the frame is in "cv_ptr->image" 

    std::vector<Rect> faces;
    Mat frame_gray;
    Mat frame(cv_ptr->image);
    resize(frame, frame, Size(160, 120));
    cvtColor( frame, frame_gray, COLOR_BGR2GRAY );
    equalizeHist( frame_gray, frame_gray );

    //t = (double)cvGetTickCount();
    //-- Detect faces
    //face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CASCADE_SCALE_IMAGE, Size(16, 12), Size(80, 60) ); // 30ms

    face_cascade.detectMultiScale( frame_gray, faces, 1.05, 2); // 35ms
    
    //t = (double)cvGetTickCount() - t;
    //printf( "detection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );

    if (faces.size() > 0) {
        //myFilter((faces[0].x + faces[0].width/2) * 4 - 320, (faces[0].y + faces[0].height/2) * 4 - 240);
        //firstBodyFound = 1;
        before_filter_x = (faces[0].x + faces[0].width/2) * 4 - 320;
        before_filter_y = (faces[0].y + faces[0].height/2) * 4 - 240;
    }
    else
    {
        //myFilter(0.0, 0.0);
        before_filter_x = 0.0;
        before_filter_y = 0.0;
    }

    myFilter(before_filter_x, before_filter_y);

    gettimeofday(&tp, NULL);
    now = tp.tv_sec * 1000000 + tp.tv_usec; 
    double t = (double) (now - start_time) / 1000000;

    printf("%f    %f    %f\n", t, before_filter_x, filter_x);

    if (1) {
        size_t i = 0;
        Point center(filter_x + 320.0, filter_y + 240);
        cv::circle(cv_ptr->image, center, 50, CV_RGB(0,255,255), 5.0);

        /*
        rectangle(cv_ptr->image,
           Point(filter_x, filter_y),
           Point(filter_x + filter_width, filter_y + filter_height),
           Scalar(0, 255, 255),
           5,
           8);

        */

        // publishing
        brray.data[0] = 0.0; // x: 0.0 (depth)
        brray.data[1] = -filter_x; // y: -320 ~ +320
        brray.data[2] = -filter_y; // z: -240 ~ +240
        brray.data[3] = 0.0;
        brray.data[4] = 0.0;

        target_xywh_pub_.publish(brray);

// control base to head to the target
        geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
	cmd->angular.z = -brray.data[1]/320 * 1.5; 
        cmd_pub_.publish(cmd);
    }

    // Draw an example circle on the video stream
    //if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
    //  cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));

    // Update GUI Window
    //cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    //cv::waitKey(3);
    
    // Output modified video stream
    image_pub_.publish(cv_ptr->toImageMsg());
  }
void morphSizeCb( int val, void *dummy )
{
  geometry_msgs::Point msg;
  msg.x = msg.y = val;
  morphSizePub.publish(msg);
}
int my_callback(int data_type, int data_len, char *content)
{
    g_lock.enter();

    /* image data */
    if (e_image == data_type && NULL != content)
    {
        image_data* data = (image_data*)content;

        if ( data->m_greyscale_image_left[CAMERA_ID] ) {
            memcpy(g_greyscale_image_left.data, data->m_greyscale_image_left[CAMERA_ID], IMAGE_SIZE);
            imshow("left",  g_greyscale_image_left);
            // publish left greyscale image
            cv_bridge::CvImage left_8;
            g_greyscale_image_left.copyTo(left_8.image);
            left_8.header.frame_id  = "guidance";
            left_8.header.stamp	= ros::Time::now();
            left_8.encoding		= sensor_msgs::image_encodings::MONO8;
            left_image_pub.publish(left_8.toImageMsg());
        }
        if ( data->m_greyscale_image_right[CAMERA_ID] ) {
            memcpy(g_greyscale_image_right.data, data->m_greyscale_image_right[CAMERA_ID], IMAGE_SIZE);
            imshow("right", g_greyscale_image_right);
            // publish right greyscale image
            cv_bridge::CvImage right_8;
            g_greyscale_image_right.copyTo(right_8.image);
            right_8.header.frame_id  = "guidance";
            right_8.header.stamp	 = ros::Time::now();
            right_8.encoding  	 = sensor_msgs::image_encodings::MONO8;
            right_image_pub.publish(right_8.toImageMsg());
        }
        if ( data->m_depth_image[CAMERA_ID] ) {
            memcpy(g_depth.data, data->m_depth_image[CAMERA_ID], IMAGE_SIZE * 2);
            g_depth.convertTo(depth8, CV_8UC1);
            imshow("depth", depth8);
            //publish depth image
            cv_bridge::CvImage depth_16;
            g_depth.copyTo(depth_16.image);
            depth_16.header.frame_id  = "guidance";
            depth_16.header.stamp	  = ros::Time::now();
            depth_16.encoding	  = sensor_msgs::image_encodings::MONO16;
            depth_image_pub.publish(depth_16.toImageMsg());
        }

        key = waitKey(1);
    }

    /* imu */
    if ( e_imu == data_type && NULL != content )
    {
        imu *imu_data = (imu*)content;
        printf( "frame index: %d, stamp: %d\n", imu_data->frame_index, imu_data->time_stamp );
        printf( "imu: [%f %f %f %f %f %f %f]\n", imu_data->acc_x, imu_data->acc_y, imu_data->acc_z, imu_data->q[0], imu_data->q[1], imu_data->q[2], imu_data->q[3] );

        // publish imu data
        geometry_msgs::TransformStamped g_imu;
        g_imu.header.frame_id = "guidance";
        g_imu.header.stamp    = ros::Time::now();
        g_imu.transform.translation.x = imu_data->acc_x;
        g_imu.transform.translation.y = imu_data->acc_y;
        g_imu.transform.translation.z = imu_data->acc_z;
        g_imu.transform.rotation.w = imu_data->q[0];
        g_imu.transform.rotation.x = imu_data->q[1];
        g_imu.transform.rotation.y = imu_data->q[2];
        g_imu.transform.rotation.z = imu_data->q[3];
        imu_pub.publish(g_imu);
    }
    /* velocity */
    if ( e_velocity == data_type && NULL != content )
    {
        velocity *vo = (velocity*)content;
        printf( "frame index: %d, stamp: %d\n", vo->frame_index, vo->time_stamp );
        printf( "vx:%f vy:%f vz:%f\n", 0.001f * vo->vx, 0.001f * vo->vy, 0.001f * vo->vz );

        // publish velocity
        geometry_msgs::Vector3Stamped g_vo;
        g_vo.header.frame_id = "guidance";
        g_vo.header.stamp    = ros::Time::now();
        g_vo.vector.x = 0.001f * vo->vx;
        g_vo.vector.y = 0.001f * vo->vy;
        g_vo.vector.z = 0.001f * vo->vz;
        velocity_pub.publish(g_vo);
    }

    /* obstacle distance */
    if ( e_obstacle_distance == data_type && NULL != content )
    {
        obstacle_distance *oa = (obstacle_distance*)content;
        printf( "frame index: %d, stamp: %d\n", oa->frame_index, oa->time_stamp );
        printf( "obstacle distance:" );
        for ( int i = 0; i < CAMERA_PAIR_NUM; ++i )
        {
            printf( " %f ", 0.01f * oa->distance[i] );
        }
        printf( "\n" );

        // publish obstacle distance
        sensor_msgs::LaserScan g_oa;
        g_oa.ranges.resize(CAMERA_PAIR_NUM);
        g_oa.header.frame_id = "guidance";
        g_oa.header.stamp    = ros::Time::now();
        for ( int i = 0; i < CAMERA_PAIR_NUM; ++i )
            g_oa.ranges[i] = 0.01f * oa->distance[i];
        obstacle_distance_pub.publish(g_oa);
    }

    /* ultrasonic */
    if ( e_ultrasonic == data_type && NULL != content )
    {
        ultrasonic_data *ultrasonic = (ultrasonic_data*)content;
        printf( "frame index: %d, stamp: %d\n", ultrasonic->frame_index, ultrasonic->time_stamp );
        for ( int d = 0; d < CAMERA_PAIR_NUM; ++d )
        {
            printf( "ultrasonic distance: %f, reliability: %d\n", ultrasonic->ultrasonic[d] * 0.001f, (int)ultrasonic->reliability[d] );
        }

        // publish ultrasonic data
        sensor_msgs::LaserScan g_ul;
        g_ul.ranges.resize(CAMERA_PAIR_NUM);
        g_ul.intensities.resize(CAMERA_PAIR_NUM);
        g_ul.header.frame_id = "guidance";
        g_ul.header.stamp    = ros::Time::now();
        for ( int d = 0; d < CAMERA_PAIR_NUM; ++d ) {
            g_ul.ranges[d] = 0.001f * ultrasonic->ultrasonic[d];
            g_ul.intensities[d] = 1.0 * ultrasonic->reliability[d];
        }
        ultrasonic_pub.publish(g_ul);
    }

    if(e_motion == data_type && NULL!=content) {
        motion* m=(motion*)content;
        printf("frame index: %d, stamp: %d\n", m->frame_index, m->time_stamp);
        printf("(px,py,pz)=(%.2f,%.2f,%.2f)\n", m->position_in_global_x, m->position_in_global_y, m->position_in_global_z);

        // publish position
        geometry_msgs::Vector3Stamped g_pos;
        g_pos.header.frame_id = "guidance";
        g_pos.header.stamp = ros::Time::now();
        g_pos.vector.x = m->position_in_global_x;
        g_pos.vector.y = m->position_in_global_y;
        g_pos.vector.z = m->position_in_global_z;
        position_pub.publish(g_pos);
    }

    g_lock.leave();
    g_event.set_event();

    return 0;
}
Example #6
0
int getNumRGBSubscribers()
{
    return realsense_reg_points_pub.getNumSubscribers() + realsense_rgb_image_pub.getNumSubscribers();
}
 void tell_want_to_dance()
 {
     int sentence_num = rand() % NUM_OF_SENTENCES;
     etts_msg.text = sentences_to_dance[sentence_num];
     etts_say_text.publish(etts_msg);
 }
void velocityCallback(const gen2_motor_driver::pid_plot &pid_plot)
{
	
	//Calculate the time between messages for the wheel velocity function.
	last_time = current_time;
	current_time = ros::Time::now();
	
	double vx = ((pid_plot.left_vel/100)+(pid_plot.right_vel/100))/2;
	double vy = 0;
  	double vth = ((pid_plot.right_vel/100)-(pid_plot.left_vel/100))/(wheel_base/100);

	//compute odometry in a typical way given the velocities of the robot
	double dt = (current_time - last_time).toSec();
	double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    	double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    	double delta_th = vth * dt;

    	x += delta_x;
    	y += delta_y;
    	th += (delta_th * encoder_scale_correction);

	//since all odometry is 6DOF we'll need a quaternion created from yaw
    	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    	//first, we'll publish the transform over tf
    	/*odom_trans.header.stamp = current_time;
    	odom_trans.header.frame_id = "odom";
    	odom_trans.child_frame_id = "base_footprint";

    	odom_trans.transform.translation.x = x;
    	odom_trans.transform.translation.y = y;
    	odom_trans.transform.translation.z = 0.0;
    	odom_trans.transform.rotation = odom_quat;*/

    	

    	//next, we'll publish the odometry message over ROS
    	nav_msgs::Odometry odom;
    	odom.header.stamp = current_time;
    	odom.header.frame_id = "odom";

    	//set the position
    	odom.pose.pose.position.x = x;
    	odom.pose.pose.position.y = y;
    	odom.pose.pose.position.z = 0.0;
    	odom.pose.pose.orientation = odom_quat;

	boost::array<double, 36> covariance = {1e-10, 0.0, 0.0, 0.0, 0.0, 0.0,
			       0.0, 0.001, 1e-10, 0.0, 0.0, 0.0,
			       0.0, 0.0, 1e6, 0.0, 0.0, 0.0,
			       0.0, 0.0, 0.0, 1e6, 0.0, 0.0,
 			       0.0, 0.0, 0.0, 0.0, 1e6, 0.0,
			       0.0, 0.0, 0.0, 0.0, 0.0, 1e-10};

	odom.pose.covariance = covariance;
	odom.twist.covariance = covariance;
    	//set the velocity
    	odom.child_frame_id = "base_footprint";
    	odom.twist.twist.linear.x = vx;
    	odom.twist.twist.linear.y = vy;
    	odom.twist.twist.angular.z = vth;

    	//publish the message
    	odom_pub.publish(odom);
}
Example #9
0
void PlaneFinder::pcCallback(const sensor_msgs::PointCloud2::ConstPtr & pc)
{


	pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2);
	pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered1 (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered3 (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>());
	pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ());

	sensor_msgs::PointCloud2 pc2;

	double height = -0.5;


	// Transformation into PCL type PointCloud2
	pcl_conversions::toPCL(*(pc), *(pcl_pc));

	//////////////////
	// Voxel filter //
	//////////////////
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (pcl_pc);
	sor.setLeafSize (0.01f, 0.01f, 0.01f);
	sor.filter (*cloud_filtered);

	// Transformation into ROS type
	//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
	//pcl_conversions::moveFromPCL(*(cloud_filtered), pc2);

	//debug2_pub.publish(pc2);


	// Transformation into PCL type PointCloud<pcl::PointXYZRGB>
	pcl::fromPCLPointCloud2(*(cloud_filtered), *(cloud_filtered1));

	if(pcl_ros::transformPointCloud("map", *(cloud_filtered1), *(cloud_filtered2), tf_)) 
	{

		////////////////////////
		// PassThrough filter //
		////////////////////////
		pcl::PassThrough<pcl::PointXYZRGB> pass;
		pass.setInputCloud (cloud_filtered2);
		pass.setFilterFieldName ("x");
		pass.setFilterLimits (-0.003, 0.9);
		//pass.setFilterLimitsNegative (true);
		pass.filter (*cloud_filtered2);

		pass.setInputCloud (cloud_filtered2);
		pass.setFilterFieldName ("y");
		pass.setFilterLimits (-0.5, 0.5);
		//pass.setFilterLimitsNegative (true);
		pass.filter (*cloud_filtered2);

		/////////////////////////
		// Planar segmentation //
		/////////////////////////
		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
		// Create the segmentation object
		pcl::SACSegmentation<pcl::PointXYZRGB> seg;
		// Optional
		seg.setOptimizeCoefficients (true);
		// Mandatory
		seg.setModelType (pcl::SACMODEL_PLANE);
		seg.setMethodType (pcl::SAC_RANSAC);
		//seg.setMaxIterations (1000);
		seg.setDistanceThreshold (0.01);

		// Create the filtering object
		pcl::ExtractIndices<pcl::PointXYZRGB> extract;

		int i = 0, nr_points = (int) cloud_filtered2->points.size ();
		// While 50% of the original cloud is still there
		while (cloud_filtered2->points.size () > 0.5 * nr_points)
		{
			// Segment the largest planar component from the remaining cloud
			seg.setInputCloud (cloud_filtered2);
			seg.segment (*inliers, *coefficients);
			if (inliers->indices.size () == 0)
			{
				std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
				break;
			}

			if( (fabs(coefficients->values[0]) < 0.02) && 
					(fabs(coefficients->values[1]) < 0.02) && 
					(fabs(coefficients->values[2]) > 0.9) )
			{

				std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
					<< coefficients->values[1] << " "
					<< coefficients->values[2] << " " 
					<< coefficients->values[3] << std::endl;

				height = coefficients->values[3];

				// Extract the inliers
				extract.setInputCloud (cloud_filtered2);
				extract.setIndices (inliers);
				extract.setNegative (false);
				extract.filter (*cloud_filtered3);

				// Transformation into ROS type
				//pcl::toPCLPointCloud2(*(cloud_filtered3), *(cloud_out));
				//pcl_conversions::moveFromPCL(*(cloud_out), pc2);

				//debug_pub.publish(pc2);
			}
			// Create the filtering object
			extract.setNegative (true);
			extract.filter (*cloud_f);
			cloud_filtered2.swap (cloud_f);
			i++;

		}

		/*
		   pass.setInputCloud (cloud_filtered2);
		   pass.setFilterFieldName ("z");
		   pass.setFilterLimits (height, 1.5);
		//pass.setFilterLimitsNegative (true);
		pass.filter (*cloud_filtered2);
		 */

		// Transformation into ROS type
		//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
		//pcl_conversions::moveFromPCL(*(cloud_out), pc2);

		//debug_pub.publish(pc2);

		/////////////////////////////////
		// Statistical Outlier Removal //
		/////////////////////////////////
		pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
		sor.setInputCloud (cloud_filtered2);
		sor.setMeanK (50);
		sor.setStddevMulThresh (1.0);
		sor.filter (*cloud_filtered2);


		//////////////////////////////////
		// Euclidian Cluster Extraction //  
		//////////////////////////////////

		// Creating the KdTree object for the search method of the extraction
		pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
		tree->setInputCloud (cloud_filtered2);

		std::vector<pcl::PointIndices> cluster_indices;
		pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
		ec.setClusterTolerance (0.02); // 2cm
		ec.setMinClusterSize (50);
		ec.setMaxClusterSize (5000);
		ec.setSearchMethod (tree);
		ec.setInputCloud (cloud_filtered2);
		ec.extract (cluster_indices);

		int j = 0;
		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
		{
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster1 (new pcl::PointCloud<pcl::PointXYZRGB>);
			for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
				cloud_cluster1->points.push_back (cloud_filtered2->points[*pit]); 
			cloud_cluster1->width = cloud_cluster1->points.size ();
			cloud_cluster1->height = 1;
			cloud_cluster1->is_dense = true;
			cloud_cluster1->header.frame_id = "/map";

			if(j == 0) {
				// Transformation into ROS type
				pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
				pcl_conversions::moveFromPCL(*(cloud_out), pc2);

				debug_pub.publish(pc2);

			}
			else {
				// Transformation into ROS type
				pcl::toPCLPointCloud2(*(cloud_cluster1), *(cloud_out));
				pcl_conversions::moveFromPCL(*(cloud_out), pc2);

				debug2_pub.publish(pc2);
			}
			j++;
		}

		// Transformation into ROS type
		//pcl::toPCLPointCloud2(*(cloud_filtered2), *(cloud_out));
		//pcl_conversions::moveFromPCL(*(cloud_out), pc2);

		//debug2_pub.publish(pc2);
		//debug2_pub.publish(*pc_map);

	}
}
Example #10
0
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
//cloud definition  
   pcl::PointCloud<pcl::PointXYZ> cloud;
   pcl::PointCloud<pcl::PointXYZ> cloud2;
   pcl::PointCloud<pcl::PointXYZ> cloud3;
   pcl::ExtractIndices<pcl::PointXYZ> extract;
   pcl::PointCloud<pcl::PointXYZ> obj_cloud1;
   pcl::PointCloud<pcl::PointXYZ> cloud_objetos;

//sensor definition 
   sensor_msgs::PointCloud2 Plano_suelo;
   sensor_msgs::PointCloud2 cloud_sobrante; 
   sensor_msgs::PointCloud2 Objeto_cercano;
   sensor_msgs::PointCloud2 Objetos_detectados;

  Fitrar_Voxel(input, 0.008);

 if (cloud_voxel.height*cloud_voxel.width>9000){ 
    if (error==true){   
	  std::cerr << "DETECTANDO PUNTOS " << cloud_voxel.height*cloud_voxel.width<< std::endl;
	  error=false;
    }
  
  pcl::fromROSMsg (cloud_voxel, cloud);
 
  // segmentacion plana
  pcl::ModelCoefficients coefficients;
  pcl::PointIndices inliers; 
  float anguloK_S;

 while(true){ 
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  seg.setOptimizeCoefficients (true); 
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);
  seg.setMaxIterations (200); 
  seg.setInputCloud (cloud.makeShared ());
  seg.segment (inliers, coefficients); 
 
  //calculo del angulo de la kinect (angulo formado por la kinect y el suelo)
  anguloK_S=acos(-coefficients.values[1]/sqrt(pow(coefficients.values[2],2)+pow(-coefficients.values[0],2)+pow(-coefficients.values[1],2)))-PI; 
  if (anguloK_S*180/PI>-75){
    //std::cerr << "Angulo kinect/suelo: " << anguloK_S*180/PI << std::endl;
    break;
  }
  else{
    //std::cerr << "Angulo kinect/suelo: " << anguloK_S*180/PI <<  "Recalculo del plan "  << std::endl;
    extract.setInputCloud (cloud.makeShared ());
    extract.setIndices (boost::make_shared<pcl::PointIndices> (inliers));
    extract.setNegative (true);
    extract.filter (cloud);  
      	
  }
 }
	  /*std::cerr << "Model coefficients: " << coefficients.values[0] << " " 
		                              << coefficients.values[1] << " "
		                              << coefficients.values[2] << " " 
		                              << coefficients.values[3] << std::endl;
	  std::cerr << "Model inliers: " << inliers.indices.size () << std::endl;*/
    
    pcl::copyPointCloud(cloud, inliers, cloud2);  
    cloud3=cloud;

    extract.setInputCloud (cloud.makeShared ());
    extract.setIndices (boost::make_shared<pcl::PointIndices> (inliers));
    extract.setNegative (true);
    extract.filter (cloud3);      	

  // Creating the KdTree object for the search method of the extraction
  //Segmentacion por agrupacion
  pcl::search::KdTree<pcl::PointXYZ> tree;
  tree.setInputCloud (cloud3.makeShared ());

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (80);
  ec.setMaxClusterSize (1500);
  ec.setSearchMethod (boost::make_shared<pcl::search::KdTree<pcl::PointXYZ> > (tree));
  ec.setInputCloud (cloud3.makeShared ());
  ec.extract (cluster_indices);

//std::cerr << "Numero de objecto detectado " << cluster_indices.size() << std::endl;


// Examine the clusters and find the nearest centroid
    float min_d = 10000;
    int memorize_indice=0;
    Eigen::Vector4f close;
    cloud_objetos.header=cloud3.header;
    for (unsigned int i = 0; i < cluster_indices.size(); ++i){
      pcl::PointCloud<pcl::PointXYZ> obj_cloud;
      pcl::copyPointCloud(cloud3, cluster_indices[i], obj_cloud); 

      cloud_objetos+=obj_cloud;

      Eigen::Vector4f xyz_centroid;
      pcl::compute3DCentroid(obj_cloud, xyz_centroid);

      if (sqrt(pow(xyz_centroid[0],2)+pow(xyz_centroid[1],2)+ pow(xyz_centroid[2],2))< min_d){ 
        min_d = sqrt(pow(xyz_centroid[0],2)+pow(xyz_centroid[1],2)+ pow(xyz_centroid[2],2));
	close [0]=xyz_centroid[2];
	close [1]=-xyz_centroid[0];
	close [2]=-xyz_centroid[1];
        memorize_indice=i;
      }
    }

//std::cerr << "Distancia minima " << min_d << std::endl;
/*std::cerr << "Nearest " << close[0] << "," 
                        << close[1] << ","
                        << close[2] << std::endl;*/


//Despues de la matriz de rotacion 
Eigen::Vector4f close_Robot;
close_Robot[0]=(close[0]*cos(-anguloK_S)+close[2]*sin(-anguloK_S))*1000-20;
close_Robot[1]=(close[1])*1000;
close_Robot[2]=(-close[0]*sin(-anguloK_S)+close[2]*cos(-anguloK_S))*1000+360;


int sizeMax=0;
if(cluster_indices.size()>0){
  pcl::copyPointCloud(cloud3, cluster_indices[memorize_indice], obj_cloud1);  
  pcl::toROSMsg(obj_cloud1, Objeto_cercano);
  sizeMax=1;
}

 Centroide.x=close_Robot[0];
 Centroide.y=close_Robot[1];
 Centroide.z=close_Robot[2];
	 
//std::cerr << "Nearest en Base_r "<<Centroide.x<<" "<< Centroide.y<<" "<<Centroide.z << std::endl;
//std::cerr << "Puntos del objeto " << obj_cloud1.size() << std::endl;
 

  //Convert the pcl cloud back to rosmsg
  pcl::toROSMsg(cloud2, Plano_suelo);
  pcl::toROSMsg(cloud3, cloud_sobrante);
  pcl::toROSMsg(cloud_objetos, Objetos_detectados);

  //Set the header of the cloud
  //cloud_filtered.header.frame_id = cloud.header.frame_id;
  cloud_sobrante.header.frame_id = cloud.header.frame_id;

  // Publish the data
  pub_Objet.publish(Centroide);
  pub_Plano_suelo.publish (Plano_suelo);
  pub_cloud_sobrante.publish (cloud_sobrante);
  pub_Objeto_cercano.publish (Objeto_cercano);  
  pub_Objetos_detectados.publish (Objetos_detectados); 
 }

 else{
	if (error==false){   
	  std::cerr << "ERROR!! NUMERO DE PUNTOS INSUFICIANTE PARA CALCULOS!! ESPERANDO MAS PUNTOS " << std::endl;
	  error=true;
	}
 }
}
Example #11
0
void VisualizationPublisherCCD::visualizationduringmotion(){

    	R_point *temp_point;
    	int temp_length;

      robotpath.points.clear();
      toolpath.points.clear();
//      mine.points.clear();
//      coilpath.points.clear();
      visitedpath.points.clear();


      int metric=M->metric;
			if(CCD->getPathLength()>0){
				temp_length=CCD->getPathLength();
				temp_point=CCD->GetRealPathRobot();
				geometry_msgs::Point p; 	
				for(int pathLength=0; pathLength<temp_length;pathLength++){
			        	p.x = temp_point[pathLength].x/metric;
    					p.y = temp_point[pathLength].y/metric;
    					p.z = 0;

    					robotpath.points.push_back(p);
				}
			//publish path			
			robotpath_pub.publish(robotpath);
		
			}
			if(CCD->getPathLength()>0){
				temp_length=CCD->getPathLength();
				temp_point=CCD->GetRealPathTool();
				geometry_msgs::Point p; 	
				for(int pathLength=0; pathLength<temp_length;pathLength++){
			        	p.x = temp_point[pathLength].x/metric;
    					p.y = temp_point[pathLength].y/metric;
    					p.z = 0;

    					toolpath.points.push_back(p);
				}
			//publish path			
			toolpath_pub.publish(toolpath);
		
			}

			if(CCD->getPathLength()>0){
			geometry_msgs::Point p; 	
			for (int i=0; i<CCD->MapSizeX; i++){
				for (int j=0; j<CCD->MapSizeY; j++){
					if ((CCD->map[i][j].presao>0)){
					  p.x=(GM->Map_Home.x+i*GM->Map_Cell_Size+0.5*GM->Map_Cell_Size)/metric;
            p.y=(GM->Map_Home.y+j*GM->Map_Cell_Size+0.5*GM->Map_Cell_Size)/metric;
					  visitedpath.points.push_back(p);
					}
				}
			}
			visitedpath_pub.publish(visitedpath);
			}

#if 0
			if(CCD->coilpeak){
				geometry_msgs::Point p; 	
			        	p.x = CCD->tocka.x;
    					p.y = CCD->tocka.y;
    					p.z = 0;

    					coilpath.points.push_back(p);
			        	p.x = CCD->tockaR.x;
    					p.y = CCD->tockaR.y;
    					p.z = 0;

    					coilpath.points.push_back(p);
			//publish path			
			coil_pub.publish(coilpath);
		
			}
			if (0)
			{
				geometry_msgs::Point p; 	
			        	p.x = 3.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = 2.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = -3.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = -2.;
    					p.y = 0;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = 0;
    					p.y = -3.;
    					p.z = 0;
    					mine.points.push_back(p);
			        	p.x = 0;
    					p.y = 2.;
    					p.z = 0;
    					mine.points.push_back(p);
			//publish path			
				mine_pub.publish(mine);
			}
#endif
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
    ros::Time whole_start = ros::Time::now();

    ros::Time declare_types_start = ros::Time::now();

    // filter
    pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
    pcl::PassThrough<sensor_msgs::PointCloud2> pass;
    pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
    pcl::ExtractIndices<pcl::PointXYZ> extract_indices2;

    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;

    // The plane and sphere coefficients
    pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
    pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());

    // The plane and sphere inliers
    pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
    pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());

    // The point clouds
    sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr plane_seg_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr sphere_RANSAC_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr rest_whole_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr whole_pc (new sensor_msgs::PointCloud2);
    sensor_msgs::PointCloud2::Ptr ball_candidate_output_cloud (new sensor_msgs::PointCloud2);

    // The PointCloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr remove_plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_RANSAC_output (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr remove_false_ball_candidate (new pcl::PointCloud<pcl::PointXYZ>);

    std::vector<int> inliers;

    ros::Time declare_types_end = ros::Time::now();

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /*
     * Pass through Filtering
     */
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ros::Time pass_start = ros::Time::now();

    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0, 2.5);
    pass.filter (*passthrough_filtered);

    passthrough_pub.publish(passthrough_filtered);

    ros::Time pass_end = ros::Time::now();

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /*
     * for plane features pcl::SACSegmentation
     */
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ros::Time plane_seg_start = ros::Time::now();

    // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
    pcl::fromROSMsg (*passthrough_filtered, *plane_seg_cloud);

    // Optional
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setAxis(Eigen::Vector3f (0, -1, 0));       // best plane should be perpendicular to z-axis
    seg.setMaxIterations (40);
    seg.setDistanceThreshold (0.05);
    seg.setInputCloud (plane_seg_cloud);
    seg.segment (*inliers_plane, *coefficients_plane);

    extract_indices.setInputCloud(plane_seg_cloud);
    extract_indices.setIndices(inliers_plane);
    extract_indices.setNegative(false);
    extract_indices.filter(*plane_seg_output);


    pcl::toROSMsg (*plane_seg_output, *plane_seg_output_cloud);
    plane_pub.publish(plane_seg_output_cloud);

    ros::Time plane_seg_end = ros::Time::now();

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /*
     * Extract rest plane and passthrough filtering
     */
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ros::Time rest_pass_start = ros::Time::now();

    // Create the filtering object
    // Remove the planar inliers, extract the rest
    extract_indices.setNegative (true);
    extract_indices.filter (*remove_plane_cloud);
    plane_seg_cloud.swap (remove_plane_cloud);

    // publish result of Removal the planar inliers, extract the rest
    pcl::toROSMsg (*plane_seg_cloud, *rest_whole_cloud);
    rest_whole_pub.publish(rest_whole_cloud);          // 'rest_whole_cloud' substituted whole_pc

    ros::Time rest_pass_end = ros::Time::now();

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ros::Time find_ball_start = ros::Time::now();
    int iter = 0;

    while(iter < 5)
    {
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /*
         * for sphere features pcl::SACSegmentation
         */
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
        pcl::fromROSMsg (*rest_whole_cloud, *sphere_cloud);

        ros::Time sphere_start = ros::Time::now();

        // Optional
        seg.setOptimizeCoefficients (false);
        // Mandatory
        seg.setModelType (pcl::SACMODEL_SPHERE);
        seg.setMethodType (pcl::SAC_RANSAC);
        seg.setMaxIterations (10000);
        seg.setDistanceThreshold (0.03);
        seg.setRadiusLimits (0.12, 0.16);
        seg.setInputCloud (sphere_cloud);
        seg.segment (*inliers_sphere, *coefficients_sphere);
        //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;


        if (inliers_sphere->indices.empty())
            std::cerr << "[[--Can't find the Sphere Candidate.--]]" << std::endl;
        else {
            extract_indices.setInputCloud(sphere_cloud);
            extract_indices.setIndices(inliers_sphere);
            extract_indices.setNegative(false);
            extract_indices.filter(*sphere_output);
            pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
            sphere_seg_pub.publish(sphere_output_cloud);          // 'sphere_output_cloud' means ball candidate point cloud
        }

        ros::Time sphere_end = ros::Time::now();

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /*
         * for sphere features pcl::SampleConsensusModelSphere
         */
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        ros::Time sphere_RANSAC_start = ros::Time::now();

        // created RandomSampleConsensus object and compute the appropriated model
        pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_output));

        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
        ransac.setDistanceThreshold (.01);
        ransac.computeModel();
        ransac.getInliers(inliers);

        // copies all inliers of the model computed to another PointCloud
        pcl::copyPointCloud<pcl::PointXYZ>(*sphere_output, inliers, *sphere_RANSAC_output);

        pcl::toROSMsg (*sphere_RANSAC_output, *sphere_RANSAC_output_cloud);

        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        /*
         * To discriminate a ball
         */
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        double w = 0;
        w = double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
            /double(sphere_output_cloud->width * sphere_output_cloud->height);

        if (w > 0.9) {
            BALL = true;
            std::cout << "can find a ball" << std::endl;
            true_ball_pub.publish(sphere_RANSAC_output_cloud);
            break;
        } else {
            BALL = false;
            std::cout << "can not find a ball" << std::endl;

            //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
            /*
             * Exclude false ball candidate
             */
            //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

            //ros::Time rest_pass_start = ros::Time::now();

            // Create the filtering object
            // Remove the planar inliers, extract the rest
            extract_indices2.setInputCloud(plane_seg_cloud);
            extract_indices2.setIndices(inliers_sphere);
            extract_indices2.setNegative (true);
            extract_indices2.filter (*remove_false_ball_candidate);
            sphere_RANSAC_output.swap (remove_false_ball_candidate);

            // publish result of Removal the planar inliers, extract the rest
            pcl::toROSMsg (*sphere_RANSAC_output, *ball_candidate_output_cloud);
            rest_ball_candidate_pub.publish(ball_candidate_output_cloud);
            rest_whole_cloud = ball_candidate_output_cloud;

        }
        iter++;
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        ros::Time sphere_RANSAC_end = ros::Time::now();

    }
    ros::Time find_ball_end = ros::Time::now();


    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    ros::Time whole_end = ros::Time::now();

    std::cout << "cloud size             : " << cloud->width * cloud->height << std::endl;
    std::cout << "plane size             : " << plane_seg_output_cloud->width * plane_seg_output_cloud->height << std::endl;
    std::cout << "rest size              : " << rest_whole_cloud->width * rest_whole_cloud->height << std::endl;
    std::cout << "sphere size            : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
    std::cout << "sphere RANSAC size     : " << sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height << "   " << sphere_RANSAC_output->points.size() << std::endl;
    std::cout << "sphereness             : " << double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
              /double(sphere_output_cloud->width * sphere_output_cloud->height) << std::endl;

    std::cout << "model coefficient      : " << coefficients_plane->values[0] << " "
              << coefficients_plane->values[1] << " "
              << coefficients_plane->values[2] << " "
              << coefficients_plane->values[3] << " " << std::endl;



    printf("\n");

    std::cout << "whole time             : " << whole_end - whole_start << " sec" << std::endl;
    std::cout << "declare types time     : " << declare_types_end - declare_types_start << " sec" << std::endl;
    std::cout << "passthrough time       : " << pass_end - pass_start << " sec" << std::endl;
    std::cout << "plane time             : " << plane_seg_end - plane_seg_start << " sec" << std::endl;
    std::cout << "rest and pass time     : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
    std::cout << "find a ball time       : " << find_ball_end - find_ball_start << " sec" << std::endl;
    //std::cout << "sphere time            : " << sphere_end - sphere_start << " sec" << std::endl;
    //std::cout << "sphere ransac time     : " << sphere_RANSAC_end - sphere_RANSAC_start << " sec" << std::endl;
    printf("\n----------------------------------------------------------------------------\n");
    printf("\n");
}
Example #13
0
    void lines_callback(const art_lrf::Lines::ConstPtr& msg_lines) {

        if ((msg_lines->theta_index.size() != 0) &&
                (msg_lines->est_rho.size() != 0) && (msg_lines->endpoints.size() !=
                        0)) {


            scan2.reset(new vector<line> ());
            line temp;
            geometry_msgs::Point32 temp2;
            geometry_msgs::Point32 temp_theta_index;
            vector<int> temp3;

            for (int i = 0; i < msg_lines->theta_index.size(); i++) {
                temp.theta_index = msg_lines->theta_index[i];
                temp.est_rho = msg_lines->est_rho[i];

                for (int j = 0; j < msg_lines->endpoints[i].points.size(); j++) {
                    temp2 = msg_lines->endpoints[i].points[j];
                    temp3.push_back(temp2.x);
                    temp3.push_back(temp2.y);
                    temp.endpoints.push_back(temp3);
                }


                scan2->push_back(temp);
                temp.endpoints.clear();
            }

            if (firstRun == true) {
                firstRun = false;
                scan1 = scan2;
                previous_translation << 0,0;
            }
            else
            {

                compare_scans(scan1, scan2, previous_est_rot, est_rot,
                              previous_translation, est_translation );

                if( pow(pow(est_translation[0],2) + pow(est_translation[1],2),0.5) >
                        WAYPOINT_THRESHOLD)
                {
                    measurement new_base;
                    for(int i=0; i<scan1->size(); i++)
                    {
                        new_base.lines.push_back(scan1->at(i));
                    }
                    new_base.x = base_translation(0) + est_translation(0);
                    new_base.y = base_translation(1) + est_translation(1);
                    new_base.yaw = base_rot;

                    // float min_dist = 2*REMATCH_CANDIDATE_THRESH; //start with
                    //	      arbitrarily high value
                    // int min_index = 0;
                    // for( int i=0; i<base_scans.size(); i++)
                    // {
                    //  float current_dist = base_scans[i].dist_from(new_base);
                    //  if( current_dist < min_dist )
                    //  {
                    //      min_index = i;
                    //      min_dist = current_dist;
                    //  }
                    // }
                    // if(min_dist < REMATCH_CANDIDATE_THRESH)
                    // {
                    //  boost::shared_ptr<vector<line> > temp_boost_scan(new vector<line>);
                    //  for(int i=0; i<base_scans[min_index].lines.size(); i++)
                    //  {
                    //      temp_boost_scan->push_back(base_scans[min_index].lines[i]);
                    //  }

                    //  int expected_rot_difference = new_base.yaw -
                    //base_scans[min_index].yaw;
                    //  int actual_rot_difference;
                    //  Vector2f expected_translation_difference;
                    //  expected_translation_difference << new_base.x -
                    //base_scans[min_index].x, new_base.y - base_scans[min_index].y;
                    //  Vector2f actual_translation_difference;

                    //  compare_scans(temp_boost_scan, scan2,
                    //                new_base.yaw - base_scans[min_index].yaw,
                    //actual_rot_difference,
                    //                expected_translation_difference,
                    //actual_translation_difference );
                    //  float translation_error =
                    // pow(pow(expected_translation_difference(0) -
                    // 	  actual_translation_difference(0), 2) +
                    //     pow(expected_translation_difference(1) -
                    // 	  actual_translation_difference(1),2), 0.5);
                    //  if( (translation_error < REMATCH_VALIDATION_THRESH) &&
                    //(abs(expected_rot_difference - actual_rot_difference) < 7))
                    //  {
                    //      //overwrite the new_base values with the difference
                    //between the waypoint and the current scan
                    //      new_base.x = base_scans[min_index].x +
                    //actual_translation_difference[0];
                    //      new_base.y = base_scans[min_index].y +
                    //actual_translation_difference[1];
                    //;	//      new_base.yaw = base_scans[min_index].yaw + actual_rot_difference;
                    //  }
                    // }



                    scan1 = scan2;
                    base_translation << new_base.x, new_base.y;
                    base_rot = new_base.yaw;
                    base_scans.push_back(new_base);
                    est_translation << 0, 0;
                    est_rot = 0;
                }

                previous_translation = est_translation;
                previous_est_rot = est_rot;


            }



            geometry_msgs::Point32 pos_msg;
            pos_msg.x = est_translation(0);
            pos_msg.y = est_translation(1);
            pos_msg.z = est_rot;

            pub_pos.publish(pos_msg);

            pos_msg_old.x = pos_msg.x;
            pos_msg_old.y = pos_msg.y;
            pos_msg_old.z = pos_msg.z;
        }



        else {
            cout << endl << "MISSED SCAN" << endl;
            pub_pos.publish(pos_msg_old);
        }

    }
void
TBK_Node::keyboardLoop()
{
    char keyboard_input;
    double max_tv = max_speed;
    double max_rv = max_turn;
    bool dirty=false;

    int speed=0;
    int turn=0;

    // get the console in raw mode
    tcgetattr(kfd, &cooked);
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &=~ (ICANON | ECHO);
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);

    puts("Reading from keyboard");
    puts("---------------------------");
    puts("q/z : increase/decrease max angular and linear speeds by 10%");
    puts("w/x : increase/decrease max linear speed by 10%");
    puts("e/c : increase/decrease max angular speed by 10%");
    puts("---------------------------");
    puts("Moving around:");
    puts("   u    i    o");
    puts("   j    k    l");
    puts("   m    ,    .");
    puts("1/2 kick left/right");
    puts("3/4 get up from forward/backwards falll");
    puts("arrow keys move head");
    puts("---------------------------");
    puts("6 stand up action");
    puts("7 sit down action");
    puts("9 start walking gait");
    puts("0 stop walking gait" );
    puts("---------------------------");
    puts("anything else : stop");
    puts("---------------------------");

    struct pollfd ufd;
    ufd.fd = kfd;
    ufd.events = POLLIN;
    for(;;)
    {
        boost::this_thread::interruption_point();

        // get the next event from the keyboard
        int num;
        if((num = poll(&ufd, 1, 250)) < 0)
        {
            perror("poll():");
            return;
        }
        else if(num > 0)
        {
            if(read(kfd, &keyboard_input, 1) < 0)
            {
                perror("read():");
                return;
            }
        }
        else
            continue;

        switch ( keyboard_input ) {
        case KEYCODE_I:
            ROS_INFO("i Go straight without rotation");
            speed = 1;
            turn = 0;
            dirty = true;
            break;
        case KEYCODE_J:
            ROS_INFO("j Turn left at a fixed position");
            speed = 0;
            turn = 1;
            dirty = true;
            break;
        case KEYCODE_COMMA:
            ROS_INFO(", Go backward without rotation");
            speed = -1;
            turn = 0;
            dirty = true;
            break;
        case KEYCODE_O:
            ROS_INFO("O Move forward with right turn");
            speed = 1;
            turn = -1;
            dirty = true;
            break;
        case KEYCODE_L:
            ROS_INFO("l Turn right at a fixed position");
            speed = 0;
            turn = -1;
            dirty = true;
            break;
        case KEYCODE_U:
            ROS_INFO("U Move forward with left turn");
            speed = 1;
            turn = 1;
            dirty = true;
            break;
        case KEYCODE_M:
            ROS_INFO("m Move backward with clockwise rotation");
            speed = -1;
            turn = -1;
            dirty = true;
            break;
        case KEYCODE_PERIOD:
            ROS_INFO(". Move backward with counter clockwise rotation");
            speed = 1;
            turn = -1;
            dirty = true;
            break;
        case KEYCODE_K:
            ROS_INFO(" k Stop");
            speed = 0;
            turn = 0;
            dirty = true;
            break;
        case KEYCODE_Q:
            ROS_INFO("q Increase angular and linear speeds by 10 percent");
            max_tv += max_tv / 10;
            max_rv += max_rv / 10;
            if(always_command)
                dirty=true;
            break;
        case KEYCODE_Z:
            ROS_INFO("Z Decrease angular and linear speeds 10 percent");
            max_tv -= max_tv / 10;
            max_rv -= max_rv / 10;
            if(always_command)
                dirty=true;
            break;
        case KEYCODE_W:
            ROS_INFO("w Increase linear speed by 10 percent");
            max_tv += max_tv / 10;
            if(always_command)
                dirty=true;
            break;
        case KEYCODE_X:
            ROS_INFO("X Decrease linear speed by 10 percent");
            max_tv -= max_tv / 10;
            if(always_command)
                dirty=true;
            break;
        case KEYCODE_E:
            ROS_INFO("e Increase angular speed by 10 percent");
            max_rv += max_rv / 10;
            if(always_command)
                dirty=true;
            break;
        case KEYCODE_C:
            ROS_INFO("C Decrease angular speed by 10 percent");
            max_rv -= max_rv / 10;
            if(always_command)
                dirty=true;
            break;
        case KEYCODE_9:
            ROS_INFO("9 enable walking");
            enable_walking_msg.data = 1;
            enable_walking_pub_.publish(enable_walking_msg);
            break;
        case KEYCODE_0:
            ROS_INFO("0 disable walking");
            enable_walking_msg.data = 0;
            enable_walking_pub_.publish(enable_walking_msg);
            break;
        case KEYCODE_1:
            ROS_INFO("1 kick left");
            action_index_msg.data = 13;
            action_pub_.publish(action_index_msg);
            break;
        case KEYCODE_2:
            ROS_INFO("2 kick right");
            action_index_msg.data = 12;
            action_pub_.publish(action_index_msg);
            break;
        case KEYCODE_3:
            ROS_INFO("3 get up from forward fall");
            action_index_msg.data = 10;
            action_pub_.publish(action_index_msg);
            break;
        case KEYCODE_4:
            ROS_INFO("4 get up from backwards fall");
            action_index_msg.data = 11;
            action_pub_.publish(action_index_msg);
            break;
        case KEYCODE_6:
            ROS_INFO("6 stand up action");
            sit_stand_msg.data = 1;
            sit_stand_pub_.publish(sit_stand_msg);
            break;
        case KEYCODE_7:
            ROS_INFO("7 sit down action");
            sit_stand_msg.data = 0;
            sit_stand_pub_.publish(sit_stand_msg);
            break;
        case KEYCODE_UP:
            ROS_INFO("head up");
            if ( tilt < 0.24 ) angle_msg.data = tilt+0.05; //User determined limit
            else angle_msg.data = tilt;
            tilt_pub_.publish(angle_msg);
            break;
        case KEYCODE_DOWN:
            ROS_INFO("head down");
            if ( tilt > -0.35 ) angle_msg.data = tilt-0.05; //User determined limit
            else angle_msg.data = tilt;
            tilt_pub_.publish(angle_msg);
            break;
        case KEYCODE_RIGHT:
            ROS_INFO("head right");
            angle_msg.data = pan-0.05;
            pan_pub_.publish(angle_msg);
            break;
        case KEYCODE_LEFT:
            ROS_INFO("head left");
            angle_msg.data = pan+0.05;
            pan_pub_.publish(angle_msg);
            break;
        case KEYCODE_ARROW_1:
            break;
        case KEYCODE_ARROW_2:
            break;
        default:
            ROS_INFO("default");
            speed = 0;
            turn = 0;
            dirty = true;
        }

       // std::cout << std::hex;
       // std::cout << (int)keyboard_input   << std::endl;
        if (dirty == true)
        {
            cmdvel.linear.x = speed * max_tv;
            cmdvel.angular.z = turn * max_rv;
            pub_.publish(cmdvel);
            dirty = false;
        }
    }
}
Example #15
0
 ///////////////////////////////////////////////////////////////////
 ///Syncronize objects  only for clouds
 //////////////////////////////////////////////////////////////////
 void Callback(const sensor_msgs::PointCloud2ConstPtr& cloudI)
 {
  
   
   ROS_INFO("cloud timestamps %i.%i ", cloudI->header.stamp.sec,cloudI->header.stamp.nsec);
      
   //////////////////////////////////////////////////////////////////
   ///for range images
   /////////////////////////////////////////////////////////////////
   
   Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
   
   pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::LASER_FRAME;
   
   float noiseLevel=0.00;
   
   float minRange = 0.0f;
   
   int borderSize = 1;
   
   
   /////////////////////////////////////////////////////////////////////
   /// point cloud 
   ///////////////////////////////////////////////////////////////////
   PointCloud<PointXYZ>::Ptr cloud_ptr (new PointCloud<PointXYZ>);
   
   
   if ((cloudI->width * cloudI->height) == 0)
     return ;
   
   ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
	     
	     (int)cloudI->width * cloudI->height,
	     
	     cloudI->header.frame_id.c_str (),
	     
	     pcl::getFieldsList (*cloudI).c_str ());
   
   
   
   
   pcl::fromROSMsg(*cloudI, *cloud_ptr);

     
   ///segment data only the part in front of
   SegmentData(cloud_ptr,cloud_filtered,cloud_nonPlanes,cloud_Plane,cloud_projected);
    
   ///////////////////////////////////////////////
   ///image range images
   //////////////////////////////////////////////
   range_image->createFromPointCloud (*cloud_filtered, pcl::deg2rad(0.2f),
				     pcl::deg2rad (360.f), pcl::deg2rad (180.0f),
				     sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
   
   /////////////////////////////////////////////////////////////////////////////////////////////
   ///planar range image TESTING
   //////////////////////////////////////////////////////////////////////////////////////////
   
   Eigen::Affine3f trans,P_affine;//does not include K
   

     trans (0,0)= 0.0f; trans (0,1)= 0.0f; trans (0,2)=-1.0f; trans(0,3)=0.0f;       

     trans (1,0)= 0.0f; trans (1,1)= 1.0f; trans (1,2)=0.0f; trans (1,3)=0.0f;

     trans (2,0)= 1.0f; trans (2,1)= 0.0f; trans (2,2)=0.0f; trans (2,3)=0.0f;

     trans (3,0)= 0.0f; trans (3,1)= 0.0f; trans (3,2)=0.0f; trans (3,3)=1.0f;
     
     
     P_affine (0,0)= 0.0187f; P_affine (0,1)= -0.8024f; P_affine (0,2)= -0.5965f; P_affine(0,3)=-0.7813f;       

     P_affine (1,0)= -0.9998f; P_affine (1,1)= -0.0168f; P_affine (1,2)=-0.0088f; P_affine (1,3)=0.1818f;

     P_affine (2,0)= -0.0030f; P_affine (2,1)= 0.5965f; P_affine (2,2)=-0.8026f; P_affine (2,3)=0.7670f;

     P_affine (3,0)= 0.0f; P_affine (3,1)= 0.0f; P_affine (3,2)=0.0f; P_affine (3,3)=1.0f;
     
     
//    0.0187   -0.8024   -0.5965   -0.7813
//    -0.9998   -0.0168   -0.0088    0.1818
//    -0.0030    0.5965   -0.8026    0.7670

   for(unsigned int i=0; i<cloud_filtered->size();i++)
   {
      PointXYZ p1,p2;
      p1=cloud_filtered->points[i];
      
      p2.x=-p1.z;
      p2.y=p1.y;
      p2.z=p1.x;
      
      cloud_trans->push_back(p2);
   }
     
  // pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, trans);
   Eigen::Affine3f pose(Eigen::Affine3f::Identity());
  // pose=pose*Translation3f(T_);
   pose=P_affine;
   ///
   range_image_planar->createFromPointCloudWithFixedSize(*cloud_filtered,768, 1024,
			//K_(0,2),K_(1,2),K_(0,0),K_(1,1),P_affine
			K_(0,2),K_(1,2),K_(0,0),K_(1,1),  Eigen::Affine3f::Identity()
                             ,pcl::RangeImage::LASER_FRAME, noiseLevel, minRange);
 
   //GenerarImagenOpenCV(range_image,im_range_current, im_range_current_norm);
   //TESTING PART with planar range image
  GenerarImagenOpenCV(range_image_planar,im_range_current, im_range_current_norm);

  applyColorMap( im_range_current_norm, falseColorsMap, COLORMAP_HOT);


   if(!firstFrame)

   {
    Mat im2 = Mat(im_range_previous_norm.rows, im_range_previous_norm.cols, im_range_previous_norm.type());
    cv::resize(im_range_current_norm, im2,im2.size(), INTER_LINEAR );
    cv::calcOpticalFlowFarneback(im2,im_range_previous_norm,flow,0.5, 3, 15, 3, 5, 1.2, 0);
   
    ///show data

    cv::cvtColor( im2 ,cflow, CV_GRAY2BGR);
   
    drawOptFlowMap(flow, cflow, 10, 1.5, Scalar(0, 255, 0));
    drawOpticalFlow_color(flow,color_flow, -1);
  
   
    
     
    //Set_range_flow(range_image,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
    ///planar range image TESTING
    Set_range_flow(range_image_planar,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
  
   ////////////////////////////////////////////////////////////////////////////////////
    ///publishing
    //////////////////////////////////////////////////////////////////////////////////
  
    cv_bridge::CvImagePtr  cv_send(new cv_bridge::CvImage);

    cv_send->image=Mat(falseColorsMap.rows, falseColorsMap.cols, CV_8UC3);
    cv_send->encoding=enc::RGB8;

   ///range image 

    falseColorsMap.copyTo(cv_send->image);

    pub_im_range.publish(cv_send->toImageMsg());


   //flow arrows

   
   cv_send->image=Mat(cflow.rows, cflow.cols, CV_8UC3);
   cflow.copyTo(cv_send->image);
   pub_im_range_flow.publish(cv_send->toImageMsg());

   
   ///color flow

   cv_send->image=Mat( color_flow.rows,  color_flow.cols, CV_8UC3);
   color_flow.copyTo(cv_send->image);

     pub_im_range_color_flow.publish(cv_send->toImageMsg());

     
   ///cloud flow
    sensor_msgs::PointCloud2 output_cloud;
   pcl::toROSMsg(*cloud_rgb,output_cloud);
   output_cloud.header.frame_id = cloudI->header.frame_id;
   pub_cloud_flow.publish(output_cloud);

   
    ////////////////////////////////////
   ///cloud complete
   ///////////////////////////////////////
   pub_cloud.publish(cloudI);
    ////////////////////////////////////////////
   ///point normal
   /////////////////////////////////////////////
   sensor_msgs::PointCloud2 output_cloud2;  
   pcl::concatenateFields(*cloud_p3d,* normals,*cloud_normals);

   

   pcl::toROSMsg(*cloud_normals,output_cloud2);
   output_cloud2.header.frame_id = cloudI->header.frame_id;
   pub_cloud_normal.publish(output_cloud2);
   
   //////////////////////////////////////////////////////////////
   ///markers
   ///////////////////////////////////////////////////////////
   visualization_msgs::Marker points,line_list;
   Publish_Marks(cloud_flow,cloud_p3d, points,line_list);
   pub_markers.publish(points);
   pub_markers.publish(line_list);
   
  }
  
    

  //create the images
  firstFrame=0;// we have read the first frame

  im_range_current_norm.copyTo(im_range_previous_norm);
  flow=Mat(im_range_current_norm.rows, im_range_current_norm.cols,CV_32FC2);
  cflow=Mat(im_range_current_norm.rows, im_range_current_norm.cols, CV_8UC3);
  
  
  
    
 }
// In this function, the Voronoi cell is calculated, integrated and the new goal point is calculated and published.
void SimpleDeployment::publish()
{
    if ( got_map_ && got_me_ ) {
        double factor = map_.info.resolution / resolution_; // zoom factor for openCV visualization

        ROS_DEBUG("SimpleDeployment: Map received, determining Voronoi cells and publishing goal");

        removeOldAgents();

        // Create variables for x-values, y-values and the maximum and minimum of these, needed for VoronoiDiagramGenerator
        float xvalues[agent_catalog_.size()];
        float yvalues[agent_catalog_.size()];
        double xmin = 0.0, xmax = 0.0, ymin = 0.0, ymax = 0.0;
        cv::Point seedPt = cv::Point(1,1);

        // Create empty image with the size of the map to draw points and voronoi diagram in
        cv::Mat vorImg = cv::Mat::zeros(map_.info.height*factor,map_.info.width*factor,CV_8UC1);

        for ( uint i = 0; i < agent_catalog_.size(); i++ ) {
            geometry_msgs::Pose pose = agent_catalog_[i].getPose();

            // Keep track of x and y values
            xvalues[i] = pose.position.x;
            yvalues[i] = pose.position.y;

            // Keep track of min and max x
            if ( pose.position.x < xmin ) {
                xmin = pose.position.x;
            } else if ( pose.position.x > xmax ) {
                xmax = pose.position.x;
            }

            // Keep track of min and max y
            if ( pose.position.y < ymin ) {
                ymin = pose.position.y;
            } else if ( pose.position.y > ymax ) {
                ymax = pose.position.y;
            }

            // Store point as seed point if it represents this agent
            if ( agent_catalog_[i].getName() == this_agent_.getName() ){
                // Scale positions in metric system column and row numbers in image
                int c = ( pose.position.x - map_.info.origin.position.x ) * factor / map_.info.resolution;
                int r = map_.info.height * factor - ( pose.position.y - map_.info.origin.position.y ) * factor / map_.info.resolution;
                cv::Point pt =  cv::Point(c,r);

                seedPt = pt;
            }
            // Draw point on image
//            cv::circle( vorImg, pt, 3, WHITE, -1, 8);
        }

        ROS_DEBUG("SimpleDeployment: creating a VDG object and generating Voronoi diagram");
        // Construct a VoronoiDiagramGenerator (VoronoiDiagramGenerator.h)
        VoronoiDiagramGenerator VDG;

        xmin = map_.info.origin.position.x; xmax = map_.info.width  * map_.info.resolution + map_.info.origin.position.x;
        ymin = map_.info.origin.position.y; ymax = map_.info.height * map_.info.resolution + map_.info.origin.position.y;

        // Generate the Voronoi diagram using the collected x and y values, the number of points, and the min and max x and y values
        VDG.generateVoronoi(xvalues,yvalues,agent_catalog_.size(),float(xmin),float(xmax),float(ymin),float(ymax));

        float x1,y1,x2,y2;

        ROS_DEBUG("SimpleDeployment: collecting line segments from the VDG object");
        // Collect the generated line segments from the VDG object
        while(VDG.getNext(x1,y1,x2,y2))
        {
            // Scale the line segment end-point coordinates to column and row numbers in image
            int c1 = ( x1 - map_.info.origin.position.x ) * factor / map_.info.resolution;
            int r1 = vorImg.rows - ( y1 - map_.info.origin.position.y ) * factor / map_.info.resolution;
            int c2 = ( x2 - map_.info.origin.position.x ) * factor / map_.info.resolution;
            int r2 = vorImg.rows - ( y2 - map_.info.origin.position.y ) * factor / map_.info.resolution;

            // Draw line segment
            cv::Point pt1 = cv::Point(c1,r1),
                      pt2 = cv::Point(c2,r2);
            cv::line(vorImg,pt1,pt2,WHITE);
        }

        ROS_DEBUG("SimpleDeployment: drawing map occupancygrid and resizing to voronoi image size");
        // Create cv image from map data and resize it to the same size as voronoi image
        cv::Mat mapImg = drawMap();
        cv::Mat viewImg(vorImg.size(),CV_8UC1);
        cv::resize(mapImg, viewImg, vorImg.size(), 0.0, 0.0, cv::INTER_NEAREST );

        // Add images together to make the total image
        cv::Mat totalImg(vorImg.size(),CV_8UC1);
        cv::bitwise_or(viewImg,vorImg,totalImg);

        cv::Mat celImg = cv::Mat::zeros(vorImg.rows+2, vorImg.cols+2, CV_8UC1);
        cv::Scalar newVal = cv::Scalar(1), upDiff = cv::Scalar(100), loDiff = cv::Scalar(256);
        cv::Rect rect;

        cv::floodFill(totalImg,celImg,seedPt,newVal,&rect,loDiff,upDiff,4 + (255 << 8) + cv::FLOODFILL_MASK_ONLY);

        // Compute the center of mass of the Voronoi cell
        cv::Moments m = moments(celImg, false);
        cv::Point centroid(m.m10/m.m00, m.m01/m.m00);

        cv::circle( celImg, centroid, 3, BLACK, -1, 8);

        // Convert seed point to celImg coordinate system (totalImg(x,y) = celImg(x+1,y+1)
        cv::Point onePt(1,1);
        centroid = centroid - onePt;

        for ( uint i = 0; i < agent_catalog_.size(); i++ ){

            int c = ( xvalues[i] - map_.info.origin.position.x ) * factor / map_.info.resolution;
            int r = map_.info.height * factor - ( yvalues[i] - map_.info.origin.position.y ) * factor / map_.info.resolution;
            cv::Point pt =  cv::Point(c,r);
            cv::circle( totalImg, pt, 3, GRAY, -1, 8);
        }

        cv::circle( totalImg, seedPt, 3, WHITE, -1, 8);
        cv::circle( totalImg, centroid, 2, WHITE, -1, 8);			//where centroid is the goal position
	
	// Due to bandwidth issues, only display this image if requested
	if (show_cells_) {
	        cv::imshow( "Voronoi Cells", totalImg );
	}
        cv::waitKey(3);

        // Scale goal position in map back to true goal position
        geometry_msgs::Pose goalPose;
//        goalPose.position.x = centroid.x * map_.info.resolution / factor + map_.info.origin.position.x;
        goalPose.position.x = centroid.x / factor + map_.info.origin.position.x;
//        goalPose.position.y = (map_.info.height - centroid.y / factor) * map_.info.resolution + map_.info.origin.position.y;
        goalPose.position.y = (map_.info.height - centroid.y / factor) + map_.info.origin.position.y;
        double phi = atan2( seedPt.y - centroid.y, centroid.x - seedPt.x );
        goalPose.orientation = tf::createQuaternionMsgFromYaw(phi);

        goal_.pose = goalPose;
        goal_.header.frame_id = "map";
        goal_.header.stamp = ros::Time::now();

        goal_pub_.publish(goal_);

//        move_base_msgs::MoveBaseGoal goal;

//        goal.target_pose.header.frame_id = "map";
//        goal.target_pose.header.stamp = ros::Time::now();

//        goal.target_pose.pose = goalPose;

//        ac_.sendGoal(goal);
    }
    else {
        ROS_DEBUG("SimpleDeployment: No map received");
    }

}
void matchThreshCb( int val, void *dummy )
{
  std_msgs::Float32 msg;
  msg.data = val / 100.0;
  matchThreshPub.publish( msg );
}
    void image_callback(const sensor_msgs::LaserScan::ConstPtr& laser, 
                        const sensor_msgs::CompressedImage::ConstPtr& image) {
        if (!this->bot.valid()) {
            return;
        }
        try {
            // Convert from ROS image msg to OpenCV matrix images
            cv_bridge::CvImagePtr cv_ptr;
          //  cv_ptr = cv_bridge::toCvCopy((sensor_msgs::Imageu) image, sensor_msgs::image_encodings::BGR8);
           // cv::Mat src = cv_ptr->image;
            cv::Mat src = cv::imdecode(image->data, 1);
            // Take hsv ranges from launch

            // OpenCV filters to find colours
            cv::Mat hsv, pink_threshold, yellow_threshold, blue_threshold, green_threshold;
            cv::cvtColor(src, hsv, CV_BGR2HSV);

            cv::inRange(hsv, cv::Scalar(pink_ranges[0],pink_ranges[2],pink_ranges[3]), cv::Scalar(pink_ranges[1],255,255), pink_threshold);
            cv::inRange(hsv, cv::Scalar(yellow_ranges[0],yellow_ranges[2],yellow_ranges[3]), cv::Scalar(yellow_ranges[1],255,255), yellow_threshold);
            cv::inRange(hsv, cv::Scalar(blue_ranges[0],blue_ranges[2],blue_ranges[3]), cv::Scalar(blue_ranges[1],255,255), blue_threshold);
            cv::inRange(hsv, cv::Scalar(green_ranges[0],green_ranges[2],green_ranges[3]), cv::Scalar(green_ranges[1],255,255), green_threshold);
            blue_threshold = cv::Scalar::all(255) - blue_threshold;
            pink_threshold = cv::Scalar::all(255) - pink_threshold;
            yellow_threshold = cv::Scalar::all(255) - yellow_threshold;
            green_threshold = cv::Scalar::all(255) - green_threshold;

            // blob detection
            cv::SimpleBlobDetector::Params params; 
            
            params.filterByArea = 1;
            params.minArea = 200;
            params.maxArea = 100000;
            params.filterByConvexity = 1;
            params.minConvexity = 0.5;
            params.filterByInertia = true;
            params.minInertiaRatio = 0.65;
            
            // find keypoints
            std::vector<cv::KeyPoint> pink_keypoints, yellow_keypoints, blue_keypoints, green_keypoints;

            cv::SimpleBlobDetector detector(params);
            detector.detect(pink_threshold, pink_keypoints);
            detector.detect(yellow_threshold, yellow_keypoints);
            detector.detect(blue_threshold, blue_keypoints);
            detector.detect(green_threshold, green_keypoints);
            
            cv::Mat blobs;
            cv::drawKeypoints (src, pink_keypoints, blobs, 
                    cv::Scalar(0,0,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

            cv::drawKeypoints (blobs, blue_keypoints, blobs, 
                    cv::Scalar(255,0,0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

            cv::drawKeypoints (blobs, green_keypoints, blobs, 
                    cv::Scalar(0,255,0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

            cv::drawKeypoints (blobs, yellow_keypoints, blobs, 
                    cv::Scalar(125,125,125), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

            // acceptable horizontal distance between the 2 colours on a pillar
            for (auto pink_pt = pink_keypoints.begin(); pink_pt != pink_keypoints.end(); pink_pt++) {
                ROS_INFO_STREAM("Pink x= " << pink_pt->pt.x);
                double xCo = pink_pt->pt.x;
                xCo = 320 - xCo;
                // TODO: you might want atan2
                double theta = atan2(xCo*tan(29 * M_PI / 180), 320.0);
		//ROS_INFO_STREAM("theta: " << (theta * 180 / M_PI));
                double lTheta = theta - laser->angle_min;
                int distance_index = lTheta / laser->angle_increment;
                /*
                double r2 = laser->ranges[distance_index];
                double r1 = 0.1;
                */
            //    double distance = sqrt( (r1 + r2*cos(theta))*(r1 + r2*cos(theta)) + (r2*sin(theta))*(r2*sin(theta)) );
                double distance = laser->ranges[distance_index];
                ROS_INFO_STREAM("BEACON IS " << distance << " FROM BOT");

                if( std::isfinite(distance) && std::isfinite(theta) && distance < 2.0 ) { 
                    distance = distance - 0.3;
            //ROS_INFO_STREAM("theta " << (theta * 180 / M_PI) << " distance " << distance);
                    search_for_match(blue_keypoints.begin(), blue_keypoints.end(), pink_pt, "blue", make_pair(distance, theta));
                    search_for_match(yellow_keypoints.begin(), yellow_keypoints.end(), pink_pt, "yellow", make_pair(distance, theta));
                    search_for_match(green_keypoints.begin(), green_keypoints.end(), pink_pt, "green", make_pair(distance, theta));
                }
            }

            // gui display
            imshow("blobs", blobs);

            bool found_all = true; 
            int count = 0;
            for (auto it = beacons.begin(); it != beacons.end(); ++it) {
                if (!it->found()) {
                    found_all = false;
                } else {
                    ROS_INFO_STREAM("Found T: " << it->top << " B: " << it->bottom << " at " << it->x << " " << it->y);
                    count++;
                }
            }

            ROS_INFO_STREAM("Found  " << count << " of " << beacons.size() << " beacons.");
            if (found_all && this->bot.valid()) {
                ROS_INFO("Found all beacons");
                
                // Send beacon message
                ass1::FoundBeacons msg;
                msg.n = beacons.size();
                msg.positions.clear();
                for (auto it = beacons.begin(); it != beacons.end(); ++it) {
                    geometry_msgs::Point point;
                    point.x = it->x;
                    point.y = it->y;
                    point.z = 0;
                    msg.positions.push_back(point);
                }
                beacons_pub.publish(msg);

                //shutdown subscriptions
                img_sub.unsubscribe();
                laser_sub.unsubscribe();
                odom_sub.shutdown();
            }

            cv::waitKey(30);
        } catch (cv_bridge::Exception& e) {
            ROS_ERROR("Could not convert from to 'bgr8'.");
        }
    }
 void stopRobot()
 {
     cmdvel.linear.x = cmdvel.angular.z = 0.0;
     pub_.publish(cmdvel);
 }
// New depth sample event varsace tieshandler
void onNewDepthSample(DepthNode node, DepthNode::NewSampleReceivedData data)
{
    //printf("Z#%u: %d %d %d\n",g_dFrames,data.vertices[500].x,data.vertices[500].y,data.vertices[500].z);
    int count = 0;
    msg->header.frame_id = "/base_link";
    // Project some 3D points in the Color Frame
    if (!g_pProjHelper)
    {
        g_pProjHelper = new ProjectionHelper (data.stereoCameraParameters);
        g_scp = data.stereoCameraParameters;
    }
    else if (g_scp != data.stereoCameraParameters)
    {
        g_pProjHelper->setStereoCameraParameters(data.stereoCameraParameters);
        g_scp = data.stereoCameraParameters;
    }

    int32_t w, h;
    FrameFormat_toResolution(data.captureConfiguration.frameFormat,&w,&h);
    int cx = w/2;
    int cy = h/2;
    msg->points.resize(w*h);

    Vertex p3DPoints[4];

    p3DPoints[0] = data.vertices[(cy-h/4)*w+cx-w/4];
    p3DPoints[1] = data.vertices[(cy-h/4)*w+cx+w/4];
    p3DPoints[2] = data.vertices[(cy+h/4)*w+cx+w/4];
    p3DPoints[3] = data.vertices[(cy+h/4)*w+cx-w/4];

    Point2D p2DPoints[4];
    g_pProjHelper->get2DCoordinates ( p3DPoints, p2DPoints, 4, CAMERA_PLANE_COLOR);

    g_dFrames++;

    msg->height = h;
    cloud.height = h;
    msg->width = w;
    cloud.width = w;
    cloud.is_dense = true;
    cloud.points.resize(w*h);

    for(int i = 0; i < h; i++) {
        for(int j = 0; j < w; j++) {
            count++;
            msg->points.push_back (pcl::PointXYZ(data.vertices[count].x,data.vertices[count].y,data.vertices[count].z));
            cloud.points[count].x = data.vertices[count].x;
            cloud.points[count].y = data.vertices[count].y;
            if(data.vertices[count].z == 32001) {
                cloud.points[count].z = 0;
            } else {
                cloud.points[count].z = data.vertices[count].z;
            }
            //cloud.points[count].rgb = 0;
        }
    }
    // We now want to create a range image from the above point cloud, with a 1deg angular resolution
    float angularResolution = (float) (  1.0f * (M_PI/180.0f));  //   1.0 degree in radians
    float maxAngleWidth     = (float) (360.0f * (M_PI/180.0f));  // 360.0 degree in radians
    float maxAngleHeight    = (float) (180.0f * (M_PI/180.0f));  // 180.0 degree in radians
    Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
    pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
    float noiseLevel=0.00;
    float minRange = 0.0f;
    int borderSize = 0;
    //pcl::RangeImage rangeImage;
    //rangeImage.createFromPointCloud(cloud, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);

    pub.publish (msg);
    // Quit the main loop after 200 depth frames received
    if (g_dFrames%1== 0) {
        pcl::io::savePCDFileASCII("pcdTest.pcd", cloud);
        g_context.quit();
    }
}
void TeleopUAVController::keyboardLoop(){
  char c;
  bool dirty=false;

  // get the console in raw mode
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  intro_ = true;

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("***************************");
  puts("Press Characters Once");
  puts("***************************");
  puts("");
  puts("Use 'WASD' to translate");
  puts("Use 'T' to take off");
  puts("Use 'L' to land");
  puts("Use 'H' to hover");
  puts("Use 'QE' to yaw");
  puts("Press 'Shift' to run");
  puts("");
  puts("---------------------------");

  for(;;)
  {
    // get the next event from the keyboard
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

    cmd.linear.z = cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;

    switch(c)
    {
      // Walking
    case KEYCODE_T:
      cmd.linear.z = walk_vel;
      ROS_INFO_STREAM("TAKE OFF!!");
      dirty = true;
      break;
    case KEYCODE_L:
      cmd.linear.z = - walk_vel;
      ROS_INFO_STREAM("LAND!!");
      dirty = true;
      break;
    case KEYCODE_H:
    	ROS_INFO_STREAM("** HOVERING **");
      dirty = true;
      break;
    case KEYCODE_W:
      cmd.linear.x = walk_vel;
	ROS_INFO_STREAM("FORWARD");
      dirty = true;
      break;
    case KEYCODE_S:
      cmd.linear.x = - walk_vel;
      ROS_INFO_STREAM("BACKWARD");
      dirty = true;
      break;
    case KEYCODE_A:
      cmd.linear.y = walk_vel;
      ROS_INFO_STREAM("LEFT <-");
      dirty = true;
      break;
    case KEYCODE_D:
      cmd.linear.y = - walk_vel;
      ROS_INFO_STREAM("RIGHT ->");
      dirty = true;
      break;
    case KEYCODE_Q:
      cmd.angular.z = yaw_rate;
      dirty = true;
      break;
    case KEYCODE_E:
      cmd.angular.z = - yaw_rate;
      dirty = true;
      break;

      // Running 
    case KEYCODE_W_CAP:
      cmd.linear.x = run_vel;

      dirty = true;
      break;
    case KEYCODE_S_CAP:
      cmd.linear.x = - run_vel;
      dirty = true;
      break;
    case KEYCODE_A_CAP:
      cmd.linear.y = run_vel;
      dirty = true;
      break;
    case KEYCODE_D_CAP:
      cmd.linear.y = - run_vel;
      dirty = true;
      break;
    case KEYCODE_Q_CAP:
      cmd.angular.z = yaw_rate_run;
      dirty = true;
      break;
    case KEYCODE_E_CAP:
      cmd.angular.z = - yaw_rate_run;
      dirty = true;
      break;
    }

    
    if (dirty == true)
    {
      vel_pub_.publish(cmd);
    }

  }
}
Example #22
0
void 
align_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
{
  if (picked)
  {  
	cloudrgbptr cloud_segment (new cloudrgb());	//Segmented point cloud from msg
  	pcl::fromROSMsg(*cloud, *cloud_segment); 	//Convert msg to point cloud
  	cloudrgbptr cloud_translated (new cloudrgb());	//Translated cloud using point
	cloudrgbptr cloud_aligned (new cloudrgb());  	//Aligned output from ICP
  	sensor_msgs::PointCloud2 cloud_aligned_msg;  	//create msg to publish

	// Translate       	
	Eigen::Matrix4f Tm;
	Tm << 	1, 0, 0, picked_x,
			0, 1, 0, picked_y,
			0, 0, 1, picked_z,
			0, 0, 0, 1;
	pcl::transformPointCloud(*cloud_source, *cloud_translated, Tm);

	// ICP

	// Setup ICP
  	pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
  	icp.setEuclideanFitnessEpsilon (1e-10);
	icp.setTransformationEpsilon (1e-6);
  	icp.setMaxCorrespondenceDistance (.1);
  	icp.setMaximumIterations(50);
  	icp.setInputSource(cloud_translated);
  	icp.setInputTarget(cloud_segment);

  	//copy the source cloud
  	cloud_aligned = cloud_translated;
	Eigen::Matrix4f prev;
 
  	//Forced iteractions with ICP loop
  	for (int i = 0; i < 10; ++i)
  	{
    	PCL_INFO ("Iteration Nr. %d.\n", i);
    	cloud_translated = cloud_aligned; //when you set to pointers equal does it actaully set the point clouds equal?
    	icp.setInputSource (cloud_translated);
    	icp.align (*cloud_aligned);
	  	Tm = icp.getFinalTransformation () * Tm;
    	std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
	  	//added change to each iteration
	  	if (fabs ((icp.getLastIncrementalTransformation () - prev).sum ()) < icp.getTransformationEpsilon ())
      		icp.setMaxCorrespondenceDistance (icp.getMaxCorrespondenceDistance () - 0.001);
    	prev = icp.getLastIncrementalTransformation ();
  	}
  	std::cout << "Final Tm:" << std::endl;
  	std::cout << Tm << std::endl;	

  	//Convert the pcl cloud back to rosmsg
  	pcl::toROSMsg(*cloud_aligned, cloud_aligned_msg);
  
  	//Set the header of the cloud
  	cloud_aligned_msg.header.frame_id = cloud->header.frame_id;
  	// Publish the data
  	//You may have to set the header frame id of the cloud_filtered also
  	pub1.publish (cloud_aligned_msg);

	// Create the transform matrix between the origin and the center of the eye (when imported)
  	// NOTE:  when imported the origin is at the top of the lifting eye
  	// This is to help when the lifting eye is translated to the picked point (which will probably
  	// be at the top of the lifting eye.
  	Eigen::Matrix4f centerOffset;
  	centerOffset << 1, 0, 0, 0,
		 			0, 1, 0, 0,
					0, 0, 1, 0.027,
					0, 0, 0, 1;
	
	// copy the original transform in case it is needed to transform the cloud later.
  	Eigen::Matrix4f originalTm = Tm;
  	// calculate center of lifting eye tranform
  	Tm = centerOffset * Tm;

	tf::Vector3 origin;
  	origin.setValue(static_cast<double>(Tm(0,3)),static_cast<double>(Tm(1,3)),static_cast<double>(Tm(2,3)));
  	tf::Matrix3x3 tf3d;
  	tf3d.setValue(	static_cast<double>(Tm(0,0)), static_cast<double>(Tm(0,1)), static_cast<double>(Tm(0,2)), 
					static_cast<double>(Tm(1,0)), static_cast<double>(Tm(1,1)), static_cast<double>(Tm(1,2)), 
					static_cast<double>(Tm(2,0)), static_cast<double>(Tm(2,1)), static_cast<double>(Tm(2,2)));

  	tf::Quaternion TmQt;
  	tf3d.getRotation(TmQt);

  	tf::Transform transform;
  	transform.setOrigin(origin);
  	transform.setRotation(TmQt);
	static tf::TransformBroadcaster br;
  	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_rgb_optical_frame", "lifting_eye"));

 	// prep for marker
	tf::Quaternion markerQt;
	markerQt =tf::createQuaternionFromRPY(0,0,M_PI/2);
	markerQt = markerQt * TmQt;
	tf::Transform markerTf;
  	markerTf.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
  	markerTf.setRotation( markerQt );

 	// Marker
	visualization_msgs::Marker marker;
	marker.header.frame_id = "camera_rgb_optical_frame";
	marker.header.stamp = ros::Time();
	marker.ns = "my_namespace";
	marker.id = 0;
	marker.type = visualization_msgs::Marker::ARROW;
	marker.action = visualization_msgs::Marker::ADD;
	marker.pose.position.x = Tm(0,3);
	marker.pose.position.y = Tm(1,3);
	marker.pose.position.z = Tm(2,3);
	marker.pose.orientation.x = markerQt.getY();
	marker.pose.orientation.y = markerQt.getZ();
	marker.pose.orientation.z = markerQt.getX();
	marker.pose.orientation.w = markerQt.getW();
	marker.scale.x = 0.005;
	marker.scale.y = 0.01;
	marker.scale.z = 0.2;
	marker.color.a = 0.5;
	marker.color.r = 1.0;
	marker.color.g = 0.0;
	marker.color.b = 0.0;
	//vis_pub.publish( marker );

	
  }
}
    // Proccess the point clouds, called when subscriber gets msg
    void cloudCallback( const sensor_msgs::PointCloud2ConstPtr& msg )
    {
        ROS_INFO_STREAM("Recieved callback");
        // Basic point cloud conversions ---------------------------------------------------------------
        // Convert from ROS to PCL
        pcl::PointCloud<pcl::PointXYZRGB> cloud;
        pcl::fromROSMsg(*msg, cloud);

        // Make new point cloud that is in our working frame
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);

        // Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link"
        tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id,
                                      cloud.header.stamp, ros::Duration(1.0));
        if(!pcl_ros::transformPointCloud(std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
        {
            ROS_ERROR("Error converting to desired frame");
            return;
        }

        // Limit to things we think are roughly at the table height ------------------------------------
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filteredZ(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PassThrough<pcl::PointXYZRGB> pass;
        pass.setInputCloud(cloud_transformed);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(min_cam_dist-.05,max_cam_dist+0.05);
        pass.filter(*cloud_filteredZ);


        // Limit to things in front of the robot ---------------------------------------------------
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
        pass.setInputCloud(cloud_filteredZ);
        pass.setFilterFieldName("x");
        pass.setFilterLimits(.1,.5);
        pass.filter(*cloud_filtered);


        // Check if any points remain
        if( cloud_filtered->points.size() == 0 )
        {
            ROS_ERROR("0 points left");
            return;
        }
        else
        {
            ROS_INFO("[block detection] Filtered, %d points left", (int) cloud_filtered->points.size());
        }

        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

        int nr_points = cloud_filtered->points.size();

        // Segment cloud until there are less than 30% of points left? not sure why this is necessary
        pcl::SACSegmentation<pcl::PointXYZRGB> seg;
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple
        seg.setMaxIterations(200);
        seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier
        while(cloud_filtered->points.size() > 0.3 * nr_points)
        {
            // Segment the largest planar component from the remaining cloud (find the table)
            seg.setInputCloud(cloud_filtered);
            seg.segment(*inliers, *coefficients);

            if(inliers->indices.size() == 0)
            {
                ROS_ERROR("[block detection] Could not estimate a planar model for the given dataset.");
                return;
            }

            // Debug output - DTC
            // Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane)
            std::cerr << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << std::endl;
        }//end while

        // DTC: Removed to make compatible with PCL 1.5
        // Creating the KdTree object for the search method of the extraction
        //pcl::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
        //tree->setInputCloud(cloud_filtered);
        // Publish point cloud data
        depth_pub_.publish(cloud_filtered);

    }//end cloudCallBack
Example #24
0
void green_light(bool status)
{
    std_msgs::Bool msg;
    msg.data = status;
    _greenlight.publish(msg);
}
Example #25
0
void speak(string val)
{
    std_msgs::String msg;
    msg.data = val;
    _speak.publish(msg);
}
Example #26
0
void red_light(bool status)
{
    std_msgs::Bool msg;
    msg.data = status;
    _redlight.publish(msg);
}
Example #27
0
void Tag_follow_class::publishtwist()
{
	if(joy_automode)
    vel_pub.publish(twist);
    cout << "the parameters of the pid are " << vel_x_kp << "  and  " << vel_x_kd<< " " <<vel_y_kp << " " << vel_y_kd << endl;
 }
Example #28
0
void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
  last_laser_received_ts_ = ros::Time::now();
  if( map_ == NULL ) {
    return;
  }
  boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
  int laser_index = -1;

  // Do we have the base->base_laser Tx yet?
  if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
  {
    ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
    lasers_.push_back(new AMCLLaser(*laser_));
    lasers_update_.push_back(true);
    laser_index = frame_to_laser_.size();

    tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                             tf::Vector3(0,0,0)),
                                 ros::Time(), laser_scan->header.frame_id);
    tf::Stamped<tf::Pose> laser_pose;
    try
    {
      this->tf_->transformPose(base_frame_id_, ident, laser_pose);
    }
    catch(tf::TransformException& e)
    {
      ROS_ERROR("Couldn't transform from %s to %s, "
                "even though the message notifier is in use",
                laser_scan->header.frame_id.c_str(),
                base_frame_id_.c_str());
      return;
    }

    pf_vector_t laser_pose_v;
    laser_pose_v.v[0] = laser_pose.getOrigin().x();
    laser_pose_v.v[1] = laser_pose.getOrigin().y();
    // laser mounting angle gets computed later -> set to 0 here!
    laser_pose_v.v[2] = 0;
    lasers_[laser_index]->SetLaserPose(laser_pose_v);
    ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
              laser_pose_v.v[0],
              laser_pose_v.v[1],
              laser_pose_v.v[2]);

    frame_to_laser_[laser_scan->header.frame_id] = laser_index;
  } else {
    // we have the laser pose, retrieve laser index
    laser_index = frame_to_laser_[laser_scan->header.frame_id];
  }

  // Where was the robot when this scan was taken?
  pf_vector_t pose;
  if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                  laser_scan->header.stamp, base_frame_id_))
  {
    ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
    return;
  }


  pf_vector_t delta = pf_vector_zero();

  if(pf_init_)
  {
    // Compute change in pose
    //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
    delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
    delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
    delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

    // See if we should update the filter
    bool update = fabs(delta.v[0]) > d_thresh_ ||
                  fabs(delta.v[1]) > d_thresh_ ||
                  fabs(delta.v[2]) > a_thresh_;
    update = update || m_force_update;
    m_force_update=false;

    // Set the laser update flags
    if(update)
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;
  }

  bool force_publication = false;
  if(!pf_init_)
  {
    // Pose at last filter update
    pf_odom_pose_ = pose;

    // Filter is now initialized
    pf_init_ = true;

    // Should update sensor data
    for(unsigned int i=0; i < lasers_update_.size(); i++)
      lasers_update_[i] = true;

    force_publication = true;

    resample_count_ = 0;
  }
  // If the robot has moved, update the filter
  else if(pf_init_ && lasers_update_[laser_index])
  {
    //printf("pose\n");
    //pf_vector_fprintf(pose, stdout, "%.3f");

    AMCLOdomData odata;
    odata.pose = pose;
    // HACK
    // Modify the delta in the action data so the filter gets
    // updated correctly
    odata.delta = delta;

    // Use the action data to update the filter
    odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);

    // Pose at last filter update
    //this->pf_odom_pose = pose;
  }

  bool resampled = false;
  // If the robot has moved, update the filter
  if(lasers_update_[laser_index])
  {
    AMCLLaserData ldata;
    ldata.sensor = lasers_[laser_index];
    ldata.range_count = laser_scan->ranges.size();

    // To account for lasers that are mounted upside-down, we determine the
    // min, max, and increment angles of the laser in the base frame.
    //
    // Construct min and max angles of laser, in the base_link frame.
    tf::Quaternion q;
    q.setRPY(0.0, 0.0, laser_scan->angle_min);
    tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
    tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                      laser_scan->header.frame_id);
    try
    {
      tf_->transformQuaternion(base_frame_id_, min_q, min_q);
      tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
    }
    catch(tf::TransformException& e)
    {
      ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
               e.what());
      return;
    }

    double angle_min = tf::getYaw(min_q);
    double angle_increment = tf::getYaw(inc_q) - angle_min;

    // wrapping angle to [-pi .. pi]
    angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;

    ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);

    // Apply range min/max thresholds, if the user supplied them
    if(laser_max_range_ > 0.0)
      ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
    else
      ldata.range_max = laser_scan->range_max;
    double range_min;
    if(laser_min_range_ > 0.0)
      range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
    else
      range_min = laser_scan->range_min;
    // The AMCLLaserData destructor will free this memory
    ldata.ranges = new double[ldata.range_count][2];
    ROS_ASSERT(ldata.ranges);
    for(int i=0;i<ldata.range_count;i++)
    {
      // amcl doesn't (yet) have a concept of min range.  So we'll map short
      // readings to max range.
      if(laser_scan->ranges[i] <= range_min)
        ldata.ranges[i][0] = ldata.range_max;
      else
        ldata.ranges[i][0] = laser_scan->ranges[i];
      // Compute bearing
      ldata.ranges[i][1] = angle_min +
              (i * angle_increment);
    }

    lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);

    lasers_update_[laser_index] = false;

    pf_odom_pose_ = pose;

    // Resample the particles
    if(!(++resample_count_ % resample_interval_))
    {
      pf_update_resample(pf_);
      resampled = true;
    }

    pf_sample_set_t* set = pf_->sets + pf_->current_set;
    ROS_DEBUG("Num samples: %d\n", set->sample_count);

    // Publish the resulting cloud
    // TODO: set maximum rate for publishing
    if (!m_force_update) {
      geometry_msgs::PoseArray cloud_msg;
      cloud_msg.header.stamp = ros::Time::now();
      cloud_msg.header.frame_id = global_frame_id_;
      cloud_msg.poses.resize(set->sample_count);
      for(int i=0;i<set->sample_count;i++)
      {
        tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
                                 tf::Vector3(set->samples[i].pose.v[0],
                                           set->samples[i].pose.v[1], 0)),
                        cloud_msg.poses[i]);
      }
      particlecloud_pub_.publish(cloud_msg);
    }
  }

  if(resampled || force_publication)
  {
    // Read out the current hypotheses
    double max_weight = 0.0;
    int max_weight_hyp = -1;
    std::vector<amcl_hyp_t> hyps;
    hyps.resize(pf_->sets[pf_->current_set].cluster_count);
    for(int hyp_count = 0;
        hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
    {
      double weight;
      pf_vector_t pose_mean;
      pf_matrix_t pose_cov;
      if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
      {
        ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
        break;
      }

      hyps[hyp_count].weight = weight;
      hyps[hyp_count].pf_pose_mean = pose_mean;
      hyps[hyp_count].pf_pose_cov = pose_cov;

      if(hyps[hyp_count].weight > max_weight)
      {
        max_weight = hyps[hyp_count].weight;
        max_weight_hyp = hyp_count;
      }
    }

    if(max_weight > 0.0)
    {
      ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                hyps[max_weight_hyp].pf_pose_mean.v[0],
                hyps[max_weight_hyp].pf_pose_mean.v[1],
                hyps[max_weight_hyp].pf_pose_mean.v[2]);

      /*
         puts("");
         pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
         puts("");
       */

      geometry_msgs::PoseWithCovarianceStamped p;
      // Fill in the header
      p.header.frame_id = global_frame_id_;
      p.header.stamp = laser_scan->header.stamp;
      // Copy in the pose
      p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
      p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
      tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                            p.pose.pose.orientation);
      // Copy in the covariance, converting from 3-D to 6-D
      pf_sample_set_t* set = pf_->sets + pf_->current_set;
      for(int i=0; i<2; i++)
      {
        for(int j=0; j<2; j++)
        {
          // Report the overall filter covariance, rather than the
          // covariance for the highest-weight cluster
          //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
          p.pose.covariance[6*i+j] = set->cov.m[i][j];
        }
      }
      // Report the overall filter covariance, rather than the
      // covariance for the highest-weight cluster
      //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
      p.pose.covariance[6*5+5] = set->cov.m[2][2];

      /*
         printf("cov:\n");
         for(int i=0; i<6; i++)
         {
         for(int j=0; j<6; j++)
         printf("%6.3f ", p.covariance[6*i+j]);
         puts("");
         }
       */

      pose_pub_.publish(p);
      last_published_pose = p;

      ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
               hyps[max_weight_hyp].pf_pose_mean.v[0],
               hyps[max_weight_hyp].pf_pose_mean.v[1],
               hyps[max_weight_hyp].pf_pose_mean.v[2]);

      // subtracting base to odom from map to base and send map to odom instead
      tf::Stamped<tf::Pose> odom_to_map;
      try
      {
        tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                             tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                         hyps[max_weight_hyp].pf_pose_mean.v[1],
                                         0.0));
        tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                              laser_scan->header.stamp,
                                              base_frame_id_);
        this->tf_->transformPose(odom_frame_id_,
                                 tmp_tf_stamped,
                                 odom_to_map);
      }
      catch(tf::TransformException)
      {
        ROS_DEBUG("Failed to subtract base to odom transform");
        return;
      }

      latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                 tf::Point(odom_to_map.getOrigin()));
      latest_tf_valid_ = true;

      if (tf_broadcast_ == true)
      {
        // We want to send a transform that is good up until a
        // tolerance time so that odom can be used
        ros::Time transform_expiration = (laser_scan->header.stamp +
                                          transform_tolerance_);
        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                            transform_expiration,
                                            global_frame_id_, odom_frame_id_);
        this->tfb_->sendTransform(tmp_tf_stamped);
        sent_first_transform_ = true;
      }
    }
    else
    {
      ROS_ERROR("No pose!");
    }
  }
  else if(latest_tf_valid_)
  {
    if (tf_broadcast_ == true)
    {
      // Nothing changed, so we'll just republish the last transform, to keep
      // everybody happy.
      ros::Time transform_expiration = (laser_scan->header.stamp +
                                        transform_tolerance_);
      tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                          transform_expiration,
                                          global_frame_id_, odom_frame_id_);
      this->tfb_->sendTransform(tmp_tf_stamped);
    }

    // Is it time to save our last pose to the param server
    ros::Time now = ros::Time::now();
    if((save_pose_period.toSec() > 0.0) &&
       (now - save_pose_last_time) >= save_pose_period)
    {
      this->savePoseToServer();
      save_pose_last_time = now;
    }
  }

}
Example #29
0
void
update (std::map<std::string, tf::StampedTransform>& frame_transforms)
{
  bool transformation_healthy = true;
  tf::StampedTransform transform;
  try
  {
    listener->lookupTransform (BASE_FRAME.c_str (), TORSO_FRAME.c_str (), ros::Time (0), transform);
    frame_transforms[TORSO_FRAME] = transform;
//    std::cout << transform.getOrigin ().x () << " " << transform.getOrigin ().y () << " "
//        << transform.getOrigin ().z () << std::endl;
    //      healthy_transformation[frame_names[i]] = true;
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s",ex.what());
    //      healthy_transformation[frame_names[i]] = false;
    transformation_healthy = false;
  }

  for (uint i = 1; i < frame_names.size (); i++)
  {
    try
    {
      listener->lookupTransform (TORSO_FRAME.c_str (), frame_names[i].c_str (), ros::Time (0), transform);
      frame_transforms[frame_names[i]] = transform;
//      std::cout << transform.getOrigin ().x () << " " << transform.getOrigin ().y () << " "
//          << transform.getOrigin ().z () << std::endl;
    }
    catch (tf::TransformException ex)
    {
      ROS_ERROR("%s",ex.what());
      transformation_healthy = false;
    }
  }

  if (transformation_healthy)
  {
    //    std::cout<<"****************"<<std::endl;
    for (uint i = 0; i < frame_names.size (); i++)
    {
      if (!frame_names[i].compare (TORSO_FRAME))
      {
        gazebo_msgs::ModelState msg_model_st;
        msg_model_st.model_name = "human";
        msg_model_st.pose.position.x = frame_transforms[TORSO_FRAME].getOrigin ().x ();
        msg_model_st.pose.position.y = frame_transforms[TORSO_FRAME].getOrigin ().y ();
        msg_model_st.pose.position.z = frame_transforms[TORSO_FRAME].getOrigin ().z ();

        msg_model_st.pose.orientation.x = frame_transforms[TORSO_FRAME].getRotation ().x ();
        msg_model_st.pose.orientation.y = frame_transforms[TORSO_FRAME].getRotation ().y ();
        msg_model_st.pose.orientation.z = frame_transforms[TORSO_FRAME].getRotation ().z ();
        msg_model_st.pose.orientation.w = frame_transforms[TORSO_FRAME].getRotation ().w ();

        msg_model_st.twist.angular.x = 0;
        msg_model_st.twist.angular.y = 0;
        msg_model_st.twist.angular.z = 0;
        msg_model_st.twist.linear.x = 0;
        msg_model_st.twist.linear.y = 0;
        msg_model_st.twist.linear.z = 0;

        msg_model_st.reference_frame = "world";
        if (pub_model_st.getNumSubscribers ())
          pub_model_st.publish (msg_model_st);
        ros::spinOnce ();

        gazebo_msgs::LinkState msg_link_st;
        msg_link_st.link_name = HUMAN_NAMESPACE + TORSO_FRAME;
        msg_link_st.pose = msg_model_st.pose;
        msg_link_st.twist = msg_model_st.twist;
        msg_link_st.reference_frame = "world";

        if (pub_link_st.getNumSubscribers ())
          pub_link_st.publish (msg_link_st);
      }

      for (uint i = 1; i < frame_names.size (); i++)
      {
        gazebo_msgs::LinkState msg_link_st;
        msg_link_st.link_name = HUMAN_NAMESPACE + frame_names[i];
//        std::cout << msg_link_st.link_name << std::endl;
        msg_link_st.pose.position.x = frame_transforms[frame_names[i]].getOrigin ().x ();
        msg_link_st.pose.position.y = frame_transforms[frame_names[i]].getOrigin ().y ();
        msg_link_st.pose.position.z = frame_transforms[frame_names[i]].getOrigin ().z ();

        msg_link_st.pose.orientation.x = frame_transforms[frame_names[i]].getRotation ().x ();
        msg_link_st.pose.orientation.y = frame_transforms[frame_names[i]].getRotation ().y ();
        msg_link_st.pose.orientation.z = frame_transforms[frame_names[i]].getRotation ().z ();
        msg_link_st.pose.orientation.w = frame_transforms[frame_names[i]].getRotation ().w ();

        msg_link_st.twist.angular.x = 0;
        msg_link_st.twist.angular.y = 0;
        msg_link_st.twist.angular.z = 0;
        msg_link_st.twist.linear.x = 0;
        msg_link_st.twist.linear.y = 0;
        msg_link_st.twist.linear.z = 0;

        msg_link_st.reference_frame = "human::torso_1";
        //        std::cout << msg_link_st.reference_frame << std::endl;
        if (pub_link_st.getNumSubscribers ())
        {
          pub_link_st.publish (msg_link_st);
          ros::spinOnce ();
        }
      }
    }
    //    std::cout<<"****************"<<std::endl;
  }
}
Example #30
-1
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
  pcl::PointXYZI sampled_p;
  pcl::PointCloud<pcl::PointXYZI> scan;

  pcl::fromROSMsg(*input, scan);

  if(measurement_range != MAX_MEASUREMENT_RANGE){
    scan = removePointsByRange(scan, 0, measurement_range);
  }

  pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
  filtered_scan_ptr->header = scan.header;

  int points_num = scan.size();

  double w_total = 0.0;
  double w_step = 0.0;
  int m = 0;
  double c = 0.0;

  filter_start = std::chrono::system_clock::now();

  for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin(); item != scan.end(); item++)
  {
    w_total += item->x * item->x + item->y * item->y + item->z * item->z;
  }
  w_step = w_total / sample_num;

  pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin();
  for (m = 0; m < sample_num; m++)
  {
    while (m * w_step > c)
    {
      item++;
      c += item->x * item->x + item->y * item->y + item->z * item->z;
    }
    sampled_p.x = item->x;
    sampled_p.y = item->y;
    sampled_p.z = item->z;
    sampled_p.intensity = item->intensity;
    filtered_scan_ptr->points.push_back(sampled_p);
  }

  sensor_msgs::PointCloud2 filtered_msg;
  pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);

  filter_end = std::chrono::system_clock::now();

  filtered_msg.header = input->header;
  filtered_points_pub.publish(filtered_msg);

  points_downsampler_info_msg.header = input->header;
  points_downsampler_info_msg.filter_name = "distance_filter";
  points_downsampler_info_msg.measurement_range = measurement_range;
  points_downsampler_info_msg.original_points_size = points_num;
  points_downsampler_info_msg.filtered_points_size = std::min((int)filtered_scan_ptr->size(), points_num);
  points_downsampler_info_msg.original_ring_size = 0;
  points_downsampler_info_msg.original_ring_size = 0;
  points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0;
  points_downsampler_info_pub.publish(points_downsampler_info_msg);

  if(_output_log == true){
	  if(!ofs){
		  std::cerr << "Could not open " << filename << "." << std::endl;
		  exit(1);
	  }
	  ofs << points_downsampler_info_msg.header.seq << ","
		  << points_downsampler_info_msg.header.stamp << ","
		  << points_downsampler_info_msg.header.frame_id << ","
		  << points_downsampler_info_msg.filter_name << ","
		  << points_downsampler_info_msg.original_points_size << ","
		  << points_downsampler_info_msg.filtered_points_size << ","
		  << points_downsampler_info_msg.original_ring_size << ","
		  << points_downsampler_info_msg.filtered_ring_size << ","
		  << points_downsampler_info_msg.exe_time << ","
		  << std::endl;
  }

}