34 #include <arpa/inet.h> 40 #include <multisense_ros/RawLidarData.h> 41 #include <multisense_ros/RawLidarCal.h> 48 const uint32_t laser_cloud_step = 16;
66 tf2::Vector3 trans{T[0][3], T[1][3], T[2][3]};
77 const float Laser::EXPECTED_RATE = 40.0;
87 reinterpret_cast<Laser*
>(userDataP)->scanCallback(header);
93 reinterpret_cast<Laser*
>(userDataP)->pointCloudCallback(header);
99 const std::string& tf_prefix):
103 previous_scan_time_(0.0)
112 if (Status_Ok != status) {
113 ROS_ERROR(
"Laser: failed to query device info: %s",
114 Channel::statusString(status));
118 if (deviceInfo.
hardwareRevision != system::DeviceInfo::HARDWARE_REV_MULTISENSE_SL) {
119 ROS_INFO(
"hardware does not support a laser");
128 frame_id_ = tf_prefix +
"/head_hokuyo_frame";
131 motor_ = tf_prefix +
"/" +
"motor";
132 spindle_ = tf_prefix +
"/" +
"spindle";
133 hokuyo_ = tf_prefix +
"/" +
"hokuyo_link";
146 if (Status_Ok != status)
147 ROS_WARN(
"could not query lidar calibration (%s), using URDF defaults",
148 Channel::statusString(status));
172 point_cloud_.header.frame_id = tf_prefix +
"/left_camera_optical_frame";
178 point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
182 point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
186 point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
190 point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
203 raw_lidar_cal_pub_ = calibration_nh.advertise<multisense_ros::RawLidarCal>(
"raw_lidar_cal", 1,
true);
204 raw_lidar_data_pub_ = calibration_nh.advertise<multisense_ros::RawLidarData>(
"raw_lidar_data", 20,
211 multisense_ros::RawLidarCal ros_msg;
214 for(uint32_t i=0; i<16; ++i)
215 ros_msg.laserToSpindle[i] = calP[i];
218 for(uint32_t i=0; i<16; ++i)
219 ros_msg.cameraToSpindleFixed[i] = calP[i];
245 std::vector<geometry_msgs::TransformStamped> stamped_transforms(2);
248 stamped_transforms[0].child_frame_id =
motor_;
252 stamped_transforms[1].header.frame_id =
spindle_;
253 stamped_transforms[1].child_frame_id =
hokuyo_;
275 std::lock_guard<std::mutex> lock(
sub_lock_);
298 uint8_t *cloudP =
reinterpret_cast<uint8_t*
>(&
point_cloud_.data[0]);
299 const uint32_t pointSize = 3 *
sizeof(float);
300 const double arcRadians = 1e-6 *
static_cast<double>(header.
scanArc);
301 const double mirrorThetaStart = -arcRadians / 2.0;
306 for(uint32_t i=0; i<header.
pointCount; ++i, cloudP += laser_cloud_step) {
311 const double percent =
static_cast<double>(i) / static_cast<double>(header.
pointCount - 1);
316 const double mirrorTheta = mirrorThetaStart + percent * arcRadians;
321 const double spindleTheta = spindleAngleStart + percent * spindleAngleRange;
328 const double rangeMeters = 1e-3 *
static_cast<double>(header.
rangesP[i]);
330 tf2::Vector3(rangeMeters * std::sin(mirrorTheta), 0.0,
331 rangeMeters * std::cos(mirrorTheta)));
332 const tf2::Vector3 pointCamera =
motor_to_camera_ * (spindle_to_motor * pointMotor);
337 const float xyz[3] = {
static_cast<float>(pointCamera.getX()),
338 static_cast<float>(pointCamera.getY()),
339 static_cast<float>(pointCamera.getZ())};
341 memcpy(cloudP, &(xyz[0]), pointSize);
342 float* intensityChannel =
reinterpret_cast<float*
>(cloudP + pointSize);
343 *intensityChannel =
static_cast<float>(header.
intensitiesP[i]);
356 const ros::Time scan_time((end_absolute_time - start_absolute_time).toSec());
359 const float angle_end = 1e-6 *
static_cast<float>(header.
spindleAngleEnd);
393 const double arcRadians = 1e-6 *
static_cast<double>(header.
scanArc);
396 laser_msg_.header.stamp = start_absolute_time;
418 RawLidarData::Ptr ros_msg(
new RawLidarData);
420 ros_msg->scan_count = header.
scanId;
421 ros_msg->time_start = start_absolute_time;
422 ros_msg->time_end = end_absolute_time;
427 memcpy(&(ros_msg->distance[0]),
432 memcpy(&(ros_msg->intensity[0]),
453 spindle_rot.
setRPY(0.0, 0.0, spindle_angle);
456 return spindle_to_motor;
481 if (Status_Ok != status)
482 ROS_ERROR(
"Laser: failed to stop laser stream: %s",
483 Channel::statusString(status));
488 std::lock_guard<std::mutex> lock(
sub_lock_);
498 std::lock_guard<std::mutex> lock(
sub_lock_);
503 if (Status_Ok != status)
504 ROS_ERROR(
"Laser: failed to start laser stream: %s",
505 Channel::statusString(status));
virtual Status startStreams(DataSource mask)=0
sensor_msgs::JointState joint_states_
void publish(const boost::shared_ptr< M > &message) const
void defaultTfPublisher(const ros::TimerEvent &event)
crl::multisense::lidar::Calibration lidar_cal_
sensor_msgs::LaserScan laser_msg_
virtual Status stopStreams(DataSource mask)=0
float cameraToSpindleFixed[4][4]
virtual Status getLidarCalibration(lidar::Calibration &c)=0
ros::Publisher raw_lidar_cal_pub_
ros::Publisher raw_lidar_data_pub_
void setRPY(tf2Scalar roll, tf2Scalar pitch, tf2Scalar yaw)
virtual Status removeIsolatedCallback(image::Callback callback)=0
virtual Status addIsolatedCallback(image::Callback callback, DataSource imageSourceMask, void *userDataP=NULL)=0
void publishSpindleTransform(const float spindle_angle, const float velocity, const ros::Time &time)
crl::multisense::Channel * driver_
tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_
ros::Publisher joint_states_pub_
uint32_t hardwareRevision
sensor_msgs::PointCloud2 point_cloud_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
def normalize_angle(angle)
float laserToSpindle[4][4]
uint32_t getNumSubscribers() const
ros::Time previous_scan_time_
virtual Status getDeviceInfo(system::DeviceInfo &info)=0
void scanCallback(const crl::multisense::lidar::Header &header)
tf2::Transform getSpindleTransform(float spindle_angle)
ros::Publisher point_cloud_pub_
void pointCloudCallback(const crl::multisense::lidar::Header &header)
std::string left_camera_optical_
tf2::Transform motor_to_camera_
tf2::Transform laser_to_spindle_