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