void FootstepPlanner::profile(FootstepAStarSolver<FootstepGraph>& solver, FootstepGraph::Ptr graph)
{
JSK_ROS_INFO("open list: %lu", solver.getOpenList().size());
JSK_ROS_INFO("close list: %lu", solver.getCloseList().size());
publishText(pub_text_,
(boost::format("open_list: %lu\nclose list:%lu")
% (solver.getOpenList().size()) % (solver.getCloseList().size())).str(),
OK);
if (rich_profiling_) {
pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
solver.openListToPointCloud(open_list_cloud);
solver.closeListToPointCloud(close_list_cloud);
publishPointCloud(close_list_cloud, pub_close_list_, latest_header_);
publishPointCloud(open_list_cloud, pub_open_list_, latest_header_);
}
}
void SearchChargingPileManager::addLaserScanMsg(const sensor_msgs::LaserScanConstPtr& msg)
{
size_t index_sum = (msg->angle_max - msg->angle_min)/ msg->angle_increment;
_sourcePoints.clear();
double angle, range;
for (size_t index = 0; index <= index_sum; index++)
{
PointInfo point;
angle = msg->angle_min + index* msg->angle_increment;
range = msg->ranges[index];
point.x = range* cos(angle);
point.y = -range* sin(angle);
point.index = index;
point.angle = angle;
point.range = range;
_sourcePoints.push_back(point);
}
// ===============deal with points to find objective==================
splitLaserWithRange(); //calculate _breakedLaser...
filterSplitLaser(_breakedLaserAngle, _breakedLaserRange); //calculate _filterLaser...
changeRangetoXY(_filterLaserAngle, _filterLaserRange);
PointWithTimeStamp keyPoint;
// ====== method 1 ======
// bool flag = findKeyPointFromLines(keyPoint);
// ====== method 2 ======
bool flag = recurFindKeyPointFromLines(keyPoint);
// ===============show pointcloud============
publishPointCloud(_filterLaserAngle, _filterLaserRange, keyPoint, flag);
// ===============send packet================
if (flag)
{
UpdateDataPacket packet;
packet.objPosition = keyPoint;
packet.objPosition.x += LaserXOffsetToRobot; //change laser coordinate to robot coordinate
packet.chargeOrderFlag = _chargeOrderFlag;
packet.powerStatusFlag = _powerStatusFlag;
tf::Transform transform;
_searchFSM->update(packet, transform);
// std::cout << transform.getOrigin().x() << " " << transform.getOrigin().y() << std::endl;
_vel_msg.linear.x = _param_linear_vel* sqrt(pow(transform.getOrigin().x(), 2.0) + pow(transform.getOrigin().y(), 2.0));
_vel_msg.angular.z = (transform.getOrigin().x() > 0.05)?
_param_angular_vel* atan2(transform.getOrigin().y(), transform.getOrigin().x()) :
_param_angular_vel* transform.getRotation().getAngle();
// boost::mutex::scoped_lock lock(_ctrlCmdVelMutex);
// _isExistValidObj = true;
}
}
void LaserPublisher::readingsCB()
{
//printf("readingsCB(): %lu ms since last readingsCB() call.\n", readingsCallbackTime->mSecSince());
assert(laser);
laser->lockDevice();
publishLaserScan();
publishPointCloud();
laser->unlockDevice();
if(broadcast_tf)
transform_broadcaster.sendTransform(tf::StampedTransform(lasertf, convertArTimeToROS(laser->getLastReadingTime()), parenttfname, tfname));
//readingsCallbackTime->setToNow();
}
void rmd::Publisher::publishDepthmapAndPointCloud() const
{
publishDepthmap();
publishPointCloud();
}
void FootstepPlanner::planCB(
const jsk_footstep_msgs::PlanFootstepsGoal::ConstPtr& goal)
{
boost::mutex::scoped_lock lock(mutex_);
latest_header_ = goal->goal_footstep.header;
JSK_ROS_INFO("planCB");
// check message sanity
if (goal->initial_footstep.footsteps.size() == 0) {
JSK_ROS_ERROR("no initial footstep is specified");
as_.setPreempted();
return;
}
if (goal->goal_footstep.footsteps.size() != 2) {
JSK_ROS_ERROR("Need to specify 2 goal footsteps");
as_.setPreempted();
return;
}
if (use_pointcloud_model_ && !pointcloud_model_) {
JSK_ROS_ERROR("No pointcloud model is yet available");
as_.setPreempted();
return;
}
//ros::WallDuration timeout(goal->timeout.expectedCycleTime().toSec());
ros::WallDuration timeout(10.0);
Eigen::Vector3f footstep_size(footstep_size_x_, footstep_size_y_, 0.000001);
////////////////////////////////////////////////////////////////////
// set start state
// 0 is always start
Eigen::Affine3f start_pose;
tf::poseMsgToEigen(goal->initial_footstep.footsteps[0].pose, start_pose);
FootstepState::Ptr start(FootstepState::fromROSMsg(
goal->initial_footstep.footsteps[0],
footstep_size,
Eigen::Vector3f(resolution_x_,
resolution_y_,
resolution_theta_)));
graph_->setStartState(start);
if (project_start_state_) {
if (!graph_->projectStart()) {
JSK_ROS_ERROR("Failed to project start state");
publishText(pub_text_,
"Failed to project start",
ERROR);
as_.setPreempted();
return;
}
}
////////////////////////////////////////////////////////////////////
// set goal state
jsk_footstep_msgs::Footstep left_goal, right_goal;
for (size_t i = 0; i < goal->goal_footstep.footsteps.size(); i++) {
FootstepState::Ptr goal_state(FootstepState::fromROSMsg(
goal->goal_footstep.footsteps[i],
footstep_size,
Eigen::Vector3f(resolution_x_,
resolution_y_,
resolution_theta_)));
if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
graph_->setLeftGoalState(goal_state);
left_goal = goal->goal_footstep.footsteps[i];
}
else if (goal_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
graph_->setRightGoalState(goal_state);
right_goal = goal->goal_footstep.footsteps[i];
}
else {
JSK_ROS_ERROR("unknown goal leg");
as_.setPreempted();
return;
}
}
if (project_goal_state_) {
if (!graph_->projectGoal()) {
JSK_ROS_ERROR("Failed to project goal");
as_.setPreempted();
publishText(pub_text_,
"Failed to project goal",
ERROR);
return;
}
}
// set parameters
if (use_transition_limit_) {
graph_->setTransitionLimit(
TransitionLimitXYZRPY::Ptr(new TransitionLimitXYZRPY(
transition_limit_x_,
transition_limit_y_,
transition_limit_z_,
transition_limit_roll_,
transition_limit_pitch_,
transition_limit_yaw_)));
}
else {
graph_->setTransitionLimit(TransitionLimitXYZRPY::Ptr());
}
graph_->setLocalXMovement(local_move_x_);
graph_->setLocalYMovement(local_move_y_);
graph_->setLocalThetaMovement(local_move_theta_);
graph_->setLocalXMovementNum(local_move_x_num_);
graph_->setLocalYMovementNum(local_move_y_num_);
graph_->setLocalThetaMovementNum(local_move_theta_num_);
graph_->setPlaneEstimationMaxIterations(plane_estimation_max_iterations_);
graph_->setPlaneEstimationMinInliers(plane_estimation_min_inliers_);
graph_->setPlaneEstimationOutlierThreshold(plane_estimation_outlier_threshold_);
graph_->setSupportCheckXSampling(support_check_x_sampling_);
graph_->setSupportCheckYSampling(support_check_y_sampling_);
graph_->setSupportCheckVertexNeighborThreshold(support_check_vertex_neighbor_threshold_);
// Solver setup
FootstepAStarSolver<FootstepGraph> solver(graph_,
close_list_x_num_,
close_list_y_num_,
close_list_theta_num_,
profile_period_,
cost_weight_,
heuristic_weight_);
if (heuristic_ == "step_cost") {
solver.setHeuristic(boost::bind(&FootstepPlanner::stepCostHeuristic, this, _1, _2));
}
else if (heuristic_ == "zero") {
solver.setHeuristic(boost::bind(&FootstepPlanner::zeroHeuristic, this, _1, _2));
}
else if (heuristic_ == "straight") {
solver.setHeuristic(boost::bind(&FootstepPlanner::straightHeuristic, this, _1, _2));
}
else if (heuristic_ == "straight_rotation") {
solver.setHeuristic(boost::bind(&FootstepPlanner::straightRotationHeuristic, this, _1, _2));
}
else {
JSK_ROS_ERROR("Unknown heuristics");
as_.setPreempted();
return;
}
solver.setProfileFunction(boost::bind(&FootstepPlanner::profile, this, _1, _2));
ros::WallTime start_time = ros::WallTime::now();
std::vector<SolverNode<FootstepState, FootstepGraph>::Ptr> path = solver.solve(timeout);
ros::WallTime end_time = ros::WallTime::now();
double planning_duration = (end_time - start_time).toSec();
JSK_ROS_INFO_STREAM("took " << planning_duration << " sec");
JSK_ROS_INFO_STREAM("path: " << path.size());
if (path.size() == 0) {
JSK_ROS_ERROR("Failed to plan path");
publishText(pub_text_,
"Failed to plan",
ERROR);
as_.setPreempted();
return;
}
// Convert path to FootstepArray
jsk_footstep_msgs::FootstepArray ros_path;
ros_path.header = goal->goal_footstep.header;
for (size_t i = 0; i < path.size(); i++) {
ros_path.footsteps.push_back(*path[i]->getState()->toROSMsg());
}
// finalize path
if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
ros_path.footsteps.push_back(right_goal);
ros_path.footsteps.push_back(left_goal);
}
else if (path[path.size() - 1]->getState()->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
ros_path.footsteps.push_back(left_goal);
ros_path.footsteps.push_back(right_goal);
}
result_.result = ros_path;
as_.setSucceeded(result_);
pcl::PointCloud<pcl::PointNormal> close_list_cloud, open_list_cloud;
solver.openListToPointCloud(open_list_cloud);
solver.closeListToPointCloud(close_list_cloud);
publishPointCloud(close_list_cloud, pub_close_list_, goal->goal_footstep.header);
publishPointCloud(open_list_cloud, pub_open_list_, goal->goal_footstep.header);
publishText(pub_text_,
(boost::format("Planning took %f sec\n%lu path\nopen list: %lu\nclose list:%lu")
% planning_duration % path.size()
% open_list_cloud.points.size()
% close_list_cloud.points.size()).str(),
OK);
}
bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
Eigen::Vector3f n, C_orig;
if (!extractPointsInsideCylinder(req.request.center,
req.request.direction,
req.request.radius,
req.request.height,
inliers, n, C_orig,
1.3)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = req.request.header;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*candidate_points);
publishPointCloud(debug_candidate_points_pub_, candidate_points);
// first, to remove plane we estimate the plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(candidate_points, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size() == 0) {
NODELET_ERROR ("plane estimation failed");
return false;
}
// remove the points blonging to the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (candidate_points);
extract.setIndices (plane_inliers);
extract.setNegative (true);
extract.filter (*points_inside_pole_wo_plane);
publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane);
// normal estimation
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
ne.setInputCloud (points_inside_pole_wo_plane);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// segmentation
pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setRadiusLimits (0.01, req.request.radius * 1.2);
seg.setDistanceThreshold (0.05);
seg.setInputCloud(points_inside_pole_wo_plane);
seg.setInputNormals (cloud_normals);
seg.setMaxIterations (10000);
seg.setNormalDistanceWeight (0.1);
seg.setAxis(n);
if (req.request.eps_angle != 0.0) {
seg.setEpsAngle(req.request.eps_angle);
}
else {
seg.setEpsAngle(0.35);
}
seg.segment (*cylinder_inliers, *cylinder_coefficients);
if (cylinder_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a cylinder model for the given dataset.");
return false;
}
debug_centroid_pub_.publish(centroid);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (points_inside_pole_wo_plane);
extract.setIndices (cylinder_inliers);
extract.setNegative (false);
extract.filter (*cylinder_points);
publishPointCloud(debug_candidate_points_pub3_, cylinder_points);
Eigen::Vector3f n_prime;
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = cylinder_coefficients->values[i];
n_prime[i] = cylinder_coefficients->values[i + 3];
}
double radius = fabs(cylinder_coefficients->values[6]);
// inorder to compute centroid, we project all the points to the center line.
// and after that, get the minimum and maximum points in the coordinate system of the center line
double min_alpha = DBL_MAX;
double max_alpha = -DBL_MAX;
for (size_t i = 0; i < cylinder_points->points.size(); i++ ) {
pcl::PointXYZ q = cylinder_points->points[i];
double alpha = (q.getVector3fMap() - C_new).dot(n_prime);
if (alpha < min_alpha) {
min_alpha = alpha;
}
if (alpha > max_alpha) {
max_alpha = alpha;
}
}
// the center of cylinder
Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime;
Eigen::Vector3f n_cross = n.cross(n_prime);
if (n.dot(n_prime)) {
n_cross = - n_cross;
}
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new_prime[0];
centroid_transformed.point.y = C_new_prime[1];
centroid_transformed.point.z = C_new_prime[2];
centroid_transformed.header = req.request.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
// publish marker
visualization_msgs::Marker marker;
marker.type = visualization_msgs::Marker::CYLINDER;
marker.scale.x = radius;
marker.scale.y = radius;
marker.scale.z = (max_alpha - min_alpha);
marker.pose.position.x = C_new_prime[0];
marker.pose.position.y = C_new_prime[1];
marker.pose.position.z = C_new_prime[2];
// n_prime -> z
// n_cross.normalized() -> x
Eigen::Vector3f z_axis = n_prime.normalized();
Eigen::Vector3f y_axis = n_cross.normalized();
Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized();
Eigen::Matrix3f M;
for (size_t i = 0; i < 3; i++) {
M(i, 0) = x_axis[i];
M(i, 1) = y_axis[i];
M(i, 2) = z_axis[i];
}
Eigen::Quaternionf q (M);
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
marker.color.g = 1.0;
marker.color.a = 1.0;
marker.header = input_header_;
marker_pub_.publish(marker);
return true;
}
bool SnapIt::processModelPlane(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
// first build plane model
geometry_msgs::PolygonStamped target_plane = req.request.target_plane;
// check the size of the points
if (target_plane.polygon.points.size() < 3) {
NODELET_ERROR("not enough points included in target_plane");
return false;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
EigenVector3fVector points;
Eigen::Vector3f n, p;
if (!extractPointsInsidePlanePole(target_plane, inliers, points, n, p)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*points_inside_pole);
publishPointCloud(debug_candidate_points_pub_, points_inside_pole);
// estimate plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(points_inside_pole, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a planar model for the given dataset.");
return false;
}
// extract plane points
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (plane_inliers);
proj.setInputCloud (points_inside_pole);
proj.setModelCoefficients (plane_coefficients);
proj.filter (*plane_points);
publishPointCloud(debug_candidate_points_pub3_, plane_points);
// next, compute convexhull and centroid
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (plane_points);
chull.setDimension(2);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
if (cloud_hull->points.size() < 3) {
NODELET_ERROR("failed to estimate convex hull");
return false;
}
publishConvexHullMarker(cloud_hull);
Eigen::Vector4f C_new_4f;
pcl::compute3DCentroid(*cloud_hull, C_new_4f);
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = C_new_4f[i];
}
Eigen::Vector3f n_prime;
n_prime[0] = plane_coefficients->values[0];
n_prime[1] = plane_coefficients->values[1];
n_prime[2] = plane_coefficients->values[2];
plane_coefficients->values[3] = plane_coefficients->values[3] / n_prime.norm();
n_prime.normalize();
if (n_prime.dot(n) < 0) {
n_prime = - n_prime;
plane_coefficients->values[3] = - plane_coefficients->values[3];
}
Eigen::Vector3f n_cross = n.cross(n_prime);
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
// compute C
Eigen::Vector3f C_orig(0, 0, 0);
for (size_t i = 0; i < points.size(); i++) {
C_orig = C_orig + points[i];
}
C_orig = C_orig / points.size();
// compute C
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = target_plane.header;
debug_centroid_pub_.publish(centroid);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new[0];
centroid_transformed.point.y = C_new[1];
centroid_transformed.point.z = C_new[2];
centroid_transformed.header = target_plane.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
return true;
}
void XtionGrabber::read_thread()
{
fd_set fds;
while(!m_shouldExit)
{
FD_ZERO(&fds);
FD_SET(m_depth_fd, &fds);
FD_SET(m_color_fd, &fds);
int ret = select(std::max(m_depth_fd, m_color_fd)+1, &fds, 0, 0, 0);
if(ret < 0)
{
perror("Could not select()");
return;
}
if(FD_ISSET(m_color_fd, &fds))
{
v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_USERPTR;
if(ioctl(m_color_fd, VIDIOC_DQBUF, &buf) != 0)
{
if(errno == EAGAIN)
continue;
perror("Could not dequeue buffer");
return;
}
ColorBuffer* buffer = &m_color_buffers[buf.index];
sensor_msgs::ImagePtr img = m_color_pool->create();
img->width = m_colorWidth;
img->height = m_colorHeight;
img->step = img->width * 4;
img->data.resize(img->step * img->height);
img->header.stamp = timeFromTimeval(buf.timestamp);
img->header.frame_id = "/camera_optical";
img->encoding = sensor_msgs::image_encodings::BGRA8;
#if HAVE_LIBYUV
libyuv::ConvertToARGB(
buffer->data.data(), buffer->data.size(),
img->data.data(),
m_colorWidth*4, 0, 0, m_colorWidth, m_colorHeight, m_colorWidth, m_colorHeight,
libyuv::kRotate0, libyuv::FOURCC_UYVY
);
#else
uint32_t* dptr = (uint32_t*)img->data.data();
for(size_t y = 0; y < img->height; ++y)
{
for(size_t x = 0; x < img->width-1; x += 2)
{
unsigned char* base = &buffer->data[y*m_colorWidth*2+x*2];
float y1 = base[1];
float u = base[0];
float y2 = base[3];
float v = base[2];
uint32_t rgb1 = yuv(y1, u, v);
uint32_t rgb2 = yuv(y2, u, v);
dptr[y*img->width + x + 0] = rgb1;
dptr[y*img->width + x + 1] = rgb2;
}
}
#endif
m_lastColorImage = img;
m_lastColorSeq = buf.sequence;
m_color_info.header.stamp = img->header.stamp;
m_pub_color.publish(img, boost::make_shared<sensor_msgs::CameraInfo>(m_color_info));
if(ioctl(m_color_fd, VIDIOC_QBUF, &buffer->buf) != 0)
{
perror("Could not queue buffer");
return;
}
}
if(FD_ISSET(m_depth_fd, &fds))
{
v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_USERPTR;
if(ioctl(m_depth_fd, VIDIOC_DQBUF, &buf) != 0)
{
if(errno == EAGAIN)
continue;
perror("Could not dequeue buffer");
return;
}
DepthBuffer* buffer = &m_depth_buffers[buf.index];
buffer->image->header.stamp = timeFromTimeval(buf.timestamp);
m_lastDepthImage = buffer->image;
m_lastDepthSeq = buf.sequence;
m_depth_info.header.stamp = buffer->image->header.stamp;
m_pub_depth.publish(buffer->image, boost::make_shared<sensor_msgs::CameraInfo>(m_depth_info));
buffer->image.reset();
buffer->image = createDepthImage();
buffer->buf.m.userptr = (long unsigned int)buffer->image->data.data();
if(ioctl(m_depth_fd, VIDIOC_QBUF, &buffer->buf) != 0)
{
perror("Could not queue buffer");
return;
}
}
if(m_lastColorSeq == m_lastDepthSeq)
{
if(m_pub_cloud.getNumSubscribers() != 0)
publishPointCloud(m_lastDepthImage, &m_cloudGenerator, &m_pub_cloud);
if(m_pub_filledCloud.getNumSubscribers() != 0)
{
sensor_msgs::ImagePtr filledDepth = m_depthFiller.fillDepth(
m_lastDepthImage, m_lastColorImage
);
publishPointCloud(filledDepth, &m_filledCloudGenerator, &m_pub_filledCloud);
}
}
}
fprintf(stderr, "read thread exit now\n");
}
void LslidarN301Decoder::packetCallback(
const lslidar_n301_msgs::LslidarN301PacketConstPtr& msg) {
// ROS_WARN("packetCallBack");
// Convert the msg to the raw packet type.
const RawPacket* raw_packet = (const RawPacket*) (&(msg->data[0]));
// Check if the packet is valid
if (!checkPacketValidity(raw_packet)) return;
// Decode the packet
decodePacket(raw_packet);
// Find the start of a new revolution
// If there is one, new_sweep_start will be the index of the start firing,
// otherwise, new_sweep_start will be FIRINGS_PER_PACKET.
size_t new_sweep_start = 0;
do {
// if (firings[new_sweep_start].firing_azimuth < last_azimuth) break;
if (fabs(firings[new_sweep_start].firing_azimuth - last_azimuth) > M_PI) break;
else {
last_azimuth = firings[new_sweep_start].firing_azimuth;
++new_sweep_start;
}
} while (new_sweep_start < FIRINGS_PER_PACKET);
// ROS_WARN("new_sweep_start %d", new_sweep_start);
// The first sweep may not be complete. So, the firings with
// the first sweep will be discarded. We will wait for the
// second sweep in order to find the 0 azimuth angle.
size_t start_fir_idx = 0;
size_t end_fir_idx = new_sweep_start;
if (is_first_sweep &&
new_sweep_start == FIRINGS_PER_PACKET) {
// The first sweep has not ended yet.
return;
} else {
if (is_first_sweep) {
is_first_sweep = false;
start_fir_idx = new_sweep_start;
end_fir_idx = FIRINGS_PER_PACKET;
sweep_start_time = msg->stamp.toSec() +
FIRING_TOFFSET * (end_fir_idx-start_fir_idx) * 1e-6;
}
}
for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
for (size_t scan_idx = 0; scan_idx < SCANS_PER_FIRING; ++scan_idx) {
// Check if the point is valid.
if (!isPointInRange(firings[fir_idx].distance[scan_idx])) continue;
// Convert the point to xyz coordinate
size_t table_idx = floor(firings[fir_idx].azimuth[scan_idx]*1000.0+0.5);
//cout << table_idx << endl;
double cos_azimuth = cos_azimuth_table[table_idx];
double sin_azimuth = sin_azimuth_table[table_idx];
//double x = firings[fir_idx].distance[scan_idx] *
// cos_scan_altitude[scan_idx] * sin(firings[fir_idx].azimuth[scan_idx]);
//double y = firings[fir_idx].distance[scan_idx] *
// cos_scan_altitude[scan_idx] * cos(firings[fir_idx].azimuth[scan_idx]);
//double z = firings[fir_idx].distance[scan_idx] *
// sin_scan_altitude[scan_idx];
double x = firings[fir_idx].distance[scan_idx] *
cos_scan_altitude[scan_idx] * sin_azimuth;
double y = firings[fir_idx].distance[scan_idx] *
cos_scan_altitude[scan_idx] * cos_azimuth;
double z = firings[fir_idx].distance[scan_idx] *
sin_scan_altitude[scan_idx];
double x_coord = y;
double y_coord = -x;
double z_coord = z;
// Compute the time of the point
double time = packet_start_time +
FIRING_TOFFSET*fir_idx + DSR_TOFFSET*scan_idx;
// Remap the index of the scan
int remapped_scan_idx = scan_idx%2 == 0 ? scan_idx/2 : scan_idx/2+8;
sweep_data->scans[remapped_scan_idx].points.push_back(
lslidar_n301_msgs::LslidarN301Point());
lslidar_n301_msgs::LslidarN301Point& new_point = // new_point 为push_back最后一个的引用
sweep_data->scans[remapped_scan_idx].points[
sweep_data->scans[remapped_scan_idx].points.size()-1];
// Pack the data into point msg
new_point.time = time;
new_point.x = x_coord;
new_point.y = y_coord;
new_point.z = z_coord;
new_point.azimuth = firings[fir_idx].azimuth[scan_idx];
new_point.distance = firings[fir_idx].distance[scan_idx];
new_point.intensity = firings[fir_idx].intensity[scan_idx];
}
}
packet_start_time += FIRING_TOFFSET * (end_fir_idx-start_fir_idx);
// A new sweep begins
if (end_fir_idx != FIRINGS_PER_PACKET) {
// ROS_WARN("A new sweep begins");
// Publish the last revolution
//sweep_data->header.stamp = ros::Time(sweep_start_time);
sweep_data->header.stamp = ros::Time();
sweep_pub.publish(sweep_data);
if (publish_point_cloud) publishPointCloud();
publishScan();
sweep_data = lslidar_n301_msgs::LslidarN301SweepPtr(
new lslidar_n301_msgs::LslidarN301Sweep());
// Prepare the next revolution
sweep_start_time = msg->stamp.toSec() +
FIRING_TOFFSET * (end_fir_idx-start_fir_idx) * 1e-6;
packet_start_time = 0.0;
last_azimuth = firings[FIRINGS_PER_PACKET-1].firing_azimuth;
start_fir_idx = end_fir_idx;
end_fir_idx = FIRINGS_PER_PACKET;
for (size_t fir_idx = start_fir_idx; fir_idx < end_fir_idx; ++fir_idx) {
for (size_t scan_idx = 0; scan_idx < SCANS_PER_FIRING; ++scan_idx) {
// Check if the point is valid.
if (!isPointInRange(firings[fir_idx].distance[scan_idx])) continue;
// Convert the point to xyz coordinate
size_t table_idx = floor(firings[fir_idx].azimuth[scan_idx]*1000.0+0.5);
//cout << table_idx << endl;
double cos_azimuth = cos_azimuth_table[table_idx];
double sin_azimuth = sin_azimuth_table[table_idx];
//double x = firings[fir_idx].distance[scan_idx] *
// cos_scan_altitude[scan_idx] * sin(firings[fir_idx].azimuth[scan_idx]);
//double y = firings[fir_idx].distance[scan_idx] *
// cos_scan_altitude[scan_idx] * cos(firings[fir_idx].azimuth[scan_idx]);
//double z = firings[fir_idx].distance[scan_idx] *
// sin_scan_altitude[scan_idx];
double x = firings[fir_idx].distance[scan_idx] *
cos_scan_altitude[scan_idx] * sin_azimuth;
double y = firings[fir_idx].distance[scan_idx] *
cos_scan_altitude[scan_idx] * cos_azimuth;
double z = firings[fir_idx].distance[scan_idx] *
sin_scan_altitude[scan_idx];
double x_coord = y;
double y_coord = -x;
double z_coord = z;
// Compute the time of the point
double time = packet_start_time +
FIRING_TOFFSET*(fir_idx-start_fir_idx) + DSR_TOFFSET*scan_idx;
// Remap the index of the scan
int remapped_scan_idx = scan_idx%2 == 0 ? scan_idx/2 : scan_idx/2+8;
sweep_data->scans[remapped_scan_idx].points.push_back(
lslidar_n301_msgs::LslidarN301Point());
lslidar_n301_msgs::LslidarN301Point& new_point =
sweep_data->scans[remapped_scan_idx].points[
sweep_data->scans[remapped_scan_idx].points.size()-1];
// Pack the data into point msg
new_point.time = time;
new_point.x = x_coord;
new_point.y = y_coord;
new_point.z = z_coord;
new_point.azimuth = firings[fir_idx].azimuth[scan_idx];
new_point.distance = firings[fir_idx].distance[scan_idx];
new_point.intensity = firings[fir_idx].intensity[scan_idx];
}
}
packet_start_time += FIRING_TOFFSET * (end_fir_idx-start_fir_idx);
}
// ROS_WARN("pack end");
return;
}
