38 nh_private_(nh_private)
40 ROS_INFO (
"Starting LaserOrthoProjector");
48 nan_point_.x = std::numeric_limits<float>::quiet_NaN();
49 nan_point_.y = std::numeric_limits<float>::quiet_NaN();
50 nan_point_.z = std::numeric_limits<float>::quiet_NaN();
68 ROS_FATAL(
"use_imu and use_pose params cannot both be true");
70 ROS_FATAL(
"use_imu and use_pose params cannot both be false");
182 ROS_WARN(
"Skipping scan %s", ex.what());
193 PointCloudT::Ptr cloud =
199 for (
unsigned int i = 0; i < scan_msg->ranges.size(); i++)
201 double r = scan_msg->ranges[i];
203 if (r > scan_msg->range_min)
212 cloud->points.push_back(point);
216 cloud->width = cloud->points.size();
218 cloud->is_dense =
true;
231 base_frame_, scan_msg->header.frame_id, scan_msg->header.stamp, base_to_laser_tf);
235 ROS_WARN(
"LaserOrthoProjector: Could not get initial laser transform(%s)", ex.what());
248 for (
unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
250 double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
251 a_cos_.push_back(cos(angle));
252 a_sin_.push_back(sin(angle));
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static double getYaw(const Quaternion &bt_q)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
TFSIMD_FORCE_INLINE const tfScalar & x() const
static Quaternion createQuaternionFromYaw(double yaw)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool getParam(const std::string &key, std::string &s) const
TFSIMD_FORCE_INLINE const tfScalar & getX() const
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)