LaserScanToPointCloud(ros::NodeHandle n) : 
   n_(n),
   laser_sub_(n_, "/robot_1/base_scan", 10),
   laser_notifier_(laser_sub_,listener_, "/robot_1/base_link", 10)
 {
   laser_notifier_.registerCallback(
     boost::bind(&LaserScanToPointCloud::scanCallback, this, _1));
   laser_notifier_.setTolerance(ros::Duration(0.01));
   scan_pub_ = n_.advertise<sensor_msgs::PointCloud>("/my_cloud",1);
 }
示例#2
0
TrajGenerator(ros::NodeHandle nh) :
    nh_(nh), 
people_sub_(nh_,"people_tracker_measurements",10),
people_notifier_(people_sub_,tfl_,fixed_frame,10)
{
people_notifier_.registerCallback(boost::bind(&TrajGenerator::peopleCallback, this, _1));
      people_notifier_.setTolerance(ros::Duration(0.01));

markers_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 20);
}
	LaserScanReceiver(ros::NodeHandle node, std::string tf, std::string topic) :
          n(node),
          base_tf(tf),
          laser_topic(topic),
          laser_sub(n, topic, 10),
          laser_notifier(laser_sub,listener, tf, 10)

        {
	    laser_notifier.registerCallback(&LaserScanReceiver::scanCallback, this);
	    laser_notifier.setTolerance(ros::Duration(0.2));
	    laser_count = 0;
	    
	    pcl_pub = node.advertise<sensor_msgs::PointCloud>("laser_pcl",1);

	}
示例#4
0
			//Nodelet initialization
			virtual void onInit()
			{
				ros::NodeHandle& nh = getNodeHandle();
				ros::NodeHandle& private_nh = getPrivateNodeHandle();

				private_nh.getParam("min_height", min_height_);
				private_nh.getParam("max_height", max_height_);
				private_nh.getParam("base_frame", baseFrame);
				private_nh.getParam("laser_frame", laserFrame);
				NODELET_INFO("CloudToScanHoriz min_height: %f, max_height: %f", min_height_, max_height_);
				NODELET_INFO("CloudToScanHoriz baseFrame: %s, laserFrame: %s", baseFrame.c_str(), laserFrame.c_str());

				//Set up to process new pointCloud and tf data together
				cloudSubscriber.subscribe(nh, "cloud_in", 5);
				tfFilter = new tf::MessageFilter<PointCloud>(cloudSubscriber, tfListener, laserFrame, 1);
				tfFilter->registerCallback(boost::bind(&CloudToScanHoriz::callback, this, _1));
				tfFilter->setTolerance(ros::Duration(0.05));

				laserPublisher = nh.advertise<sensor_msgs::LaserScan>("laserScanHoriz", 10);
			};
示例#5
0
    PCFilter() :
        nh_(),
        point_cloud_processed_(true),
        tolerance_(0.04)
    {
        m_pointCloudSub = new message_filters::Subscriber<sensor_msgs::PointCloud2> (nh_, "cloud_in", 5);
        m_tfPointCloudSub = new tf::MessageFilter<sensor_msgs::PointCloud2> (*m_pointCloudSub, tf_listener_, "world", 5);
        m_tfPointCloudSub->registerCallback(boost::bind(&PCFilter::insertCloudCallback, this, _1));

        pub_pc_ = nh_.advertise<sensor_msgs::PointCloud2 >("cloud_out", 10);
    }
	ScanToPointCloud2For2Lasers(ros::NodeHandle n) :
		_nodeHandle(n),
		target_frame("/rotary_base"),
		_laser_sub(_nodeHandle, "/scan",300),
		_laser_tfMessagefilter(_laser_sub, _tfListener, "/laser", 300),
		producedPcdCount(0),
		maxToProducePcdCount(5),
		bRunning(true),
		isSkipThisCloud(true), // set it to true to skip the first cloud
		_the_last_1st_yaw(0),
		_the_last_2nd_yaw(0)
	{
		_laser_tfMessagefilter.setTolerance(ros::Duration(0.1));
		_laser_tfMessagefilter.registerCallback(boost::bind(&ScanToPointCloud2For2Lasers::scanCallback, this, _1));

		_scan_pub = _nodeHandle.advertise<sensor_msgs::PointCloud2> ("/cloud", 100);

		std::stringstream pid;
		pid << getpid();
		pcd_prefix = pid.str();

	}
    CollisionMapTest(): private_handle_("~")
    {
        done_ = false;
        num_msgs_ = 0;
        result_ = true;

        //  ROS_INFO("Private handle: %s, %s",private_handle_.getName(), private_handle_.getNamespace());

        private_handle_.param<double>("min_z_threshold",min_z_threshold_,MIN_Z_THRESHOLD);
        private_handle_.param<double>("max_z_threshold",max_z_threshold_,MAX_Z_THRESHOLD);
        private_handle_.param<int>("test_num_collision_msgs",test_num_msgs_,TEST_NUM_MSGS);

        private_handle_.param<std::string>("collision_map_frame",collision_map_frame_,COLLISION_MAP_FRAME);

        cloud_subscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionMap>(node_,COLLISION_MAP_TOPIC,50);
        cloud_notifier_ = new tf::MessageFilter<arm_navigation_msgs::CollisionMap>(*cloud_subscriber_,tf_,collision_map_frame_,50);
        cloud_notifier_->registerCallback(boost::bind(&CollisionMapTest::collisionCallback,this,_1));

    }
 HeadTransformer() : tf_(),  target_frame_("head") {
   point_sub_.subscribe(n_, "/head/pose", 10);
   tf_filter_ = new tf::MessageFilter<geometry_msgs::PoseStamped>(point_sub_, tf_, target_frame_, 10);
   tf_filter_->registerCallback( boost::bind( &HeadTransformer::msgCallback, this, _1) );
 } ;
示例#9
0
void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
{
    boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);

    //we don't want to do anything on the first call
    //which corresponds to startup
    if(first_reconfigure_call_)
    {
        first_reconfigure_call_ = false;
        default_config_ = config;
        return;
    }

    if(config.restore_defaults) {
        config = default_config_;
        //avoid looping
        config.restore_defaults = false;
    }

    d_thresh_ = config.update_min_d;
    a_thresh_ = config.update_min_a;

    resample_interval_ = config.resample_interval;

    laser_min_range_ = config.laser_min_range;
    laser_max_range_ = config.laser_max_range;

    gui_publish_period = ros::Duration(1.0/config.gui_publish_rate);
    save_pose_period = ros::Duration(1.0/config.save_pose_rate);

    transform_tolerance_.fromSec(config.transform_tolerance);

    max_beams_ = config.laser_max_beams;
    alpha1_ = config.odom_alpha1;
    alpha2_ = config.odom_alpha2;
    alpha3_ = config.odom_alpha3;
    alpha4_ = config.odom_alpha4;
    alpha5_ = config.odom_alpha5;

    z_hit_ = config.laser_z_hit;
    z_short_ = config.laser_z_short;
    z_max_ = config.laser_z_max;
    z_rand_ = config.laser_z_rand;
    sigma_hit_ = config.laser_sigma_hit;
    lambda_short_ = config.laser_lambda_short;
    laser_likelihood_max_dist_ = config.laser_likelihood_max_dist;

    if(config.laser_model_type == "beam")
        laser_model_type_ = LASER_MODEL_BEAM;
    else if(config.laser_model_type == "likelihood_field")
        laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;

    if(config.odom_model_type == "diff")
        odom_model_type_ = ODOM_MODEL_DIFF;
    else if(config.odom_model_type == "omni")
        odom_model_type_ = ODOM_MODEL_OMNI;
    else if(config.odom_model_type == "diff-corrected")
        odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;
    else if(config.odom_model_type == "omni-corrected")
        odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;

    if(config.min_particles > config.max_particles)
    {
        ROS_WARN("You've set min_particles to be less than max particles, this isn't allowed so they'll be set to be equal.");
        config.max_particles = config.min_particles;
    }

    min_particles_ = config.min_particles;
    max_particles_ = config.max_particles;
    alpha_slow_ = config.recovery_alpha_slow;
    alpha_fast_ = config.recovery_alpha_fast;
    tf_broadcast_ = config.tf_broadcast;

    pf_ = pf_alloc(min_particles_, max_particles_,
                   alpha_slow_, alpha_fast_,
                   (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                   (void *)map_);
    pf_err_ = config.kld_err;
    pf_z_ = config.kld_z;
    pf_->pop_err = pf_err_;
    pf_->pop_z = pf_z_;

    // Initialize the filter
    pf_vector_t pf_init_pose_mean = pf_vector_zero();
    pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
    pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
    pf_init_pose_mean.v[2] = tf::getYaw(last_published_pose.pose.pose.orientation);
    pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
    pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0];
    pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1];
    pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5];
    pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
    pf_init_ = false;

    // Instantiate the sensor objects
    // Odometry
    delete odom_;
    odom_ = new AMCLOdom();
    ROS_ASSERT(odom_);
    odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
    // Laser
    delete laser_;
    laser_ = new AMCLLaser(max_beams_, map_);
    ROS_ASSERT(laser_);
    if(laser_model_type_ == LASER_MODEL_BEAM)
        laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                             sigma_hit_, lambda_short_, 0.0);
    else
    {
        ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
        laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                        laser_likelihood_max_dist_);
        ROS_INFO("Done initializing likelihood field model.");
    }

    odom_frame_id_ = config.odom_frame_id;
    base_frame_id_ = config.base_frame_id;
    global_frame_id_ = config.global_frame_id;

    delete laser_scan_filter_;
    laser_scan_filter_ =
        new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,
                *tf_,
                odom_frame_id_,
                100);
    laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                         this, _1));

    initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
}
void AmclNode::runFromBag(const std::string &in_bag_fn)
{
  rosbag::Bag bag;
  bag.open(in_bag_fn, rosbag::bagmode::Read);
  std::vector<std::string> topics;
  topics.push_back(std::string("tf"));
  std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS
  topics.push_back(scan_topic_name);
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100);
  ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100);

  // Sleep for a second to let all subscribers connect
  ros::WallDuration(1.0).sleep();

  ros::WallTime start(ros::WallTime::now());

  // Wait for map
  while (ros::ok())
  {
    {
      boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
      if (map_)
      {
        ROS_INFO("Map is ready");
        break;
      }
    }
    ROS_INFO("Waiting for map...");
    ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0));
  }

  BOOST_FOREACH(rosbag::MessageInstance const msg, view)
  {
    if (!ros::ok())
    {
      break;
    }

    // Process any ros messages or callbacks at this point
    ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration());

    tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
    if (tf_msg != NULL)
    {
      tf_pub.publish(msg);
      for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii)
      {
        tf_->getBuffer().setTransform(tf_msg->transforms[ii], "rosbag_authority");
      }
      continue;
    }

    sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>();
    if (base_scan != NULL)
    {
      laser_pub.publish(msg);
      laser_scan_filter_->add(base_scan);
      if (bag_scan_period_ > ros::WallDuration(0))
      {
        bag_scan_period_.sleep();
      }
      continue;
    }

    ROS_WARN_STREAM("Unsupported message type" << msg.getTopic());
  }

  bag.close();

  double runtime = (ros::WallTime::now() - start).toSec();
  ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime);

  const geometry_msgs::Quaternion & q(last_published_pose.pose.pose.orientation);
  double yaw, pitch, roll;
  tf::Matrix3x3(tf::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw,pitch,roll);
  ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f",
            last_published_pose.pose.pose.position.x,
            last_published_pose.pose.pose.position.y,
            yaw, last_published_pose.header.stamp.toSec()
            );

  ros::shutdown();
}