LDP PSMpositionNode::rosToLDPScan(const sensor_msgs::LaserScan& scan,
const geometry_msgs::Pose2D& basePose)//***************************************************************rostoLDPScan()
{
unsigned int n = scan.ranges.size();
LDP ld = ld_alloc_new(n);
for (int i = 0; i < n; i++)
{
ld->readings[i] = scan.ranges[i];
#ifdef USE_PROJECTED_SCANS
ld->theta[i] = scan.angles[i];
#else
ld->theta[i] = scan.angle_min + (double)i * scan.angle_increment;
#endif
if (scan.ranges[i] == 0 || scan.ranges[i] > scan.range_max)
ld->valid[i] = 0;
else
ld->valid[i] = 1;
ld->cluster[i] = -1;
}
ld->min_theta = ld->theta[0];
ld->max_theta = ld->theta[n-1];
ld->odometry[0] = basePose.x;
ld->odometry[1] = basePose.y;
ld->odometry[2] = basePose.theta;
return ld;
}
bool CanonicalScan::pointCloudToLDP(const sensor_msgs::PointCloud &cloud, LDP &ldp)
{
unsigned int n = cloud.points.size();
ldp = ld_alloc_new(n);
if(n > 0)
{
for(unsigned int i = 0; i < n; i++)
{
double r = hypot(cloud.points[i].x, cloud.points[i].y);
if (r > csm_range_min_ && r < csm_range_max_)
{
ldp->valid[i] = 1;
ldp->readings[i] = r;
}
else
{
ldp->valid[i] = 0;
ldp->readings[i] = -1; // for invalid range
}
ldp->theta[i] = angles::normalize_angle_positive(
atan2(cloud.points[i].y, cloud.points[i].x) - min_theta) + min_theta;
ldp->cluster[i] = -1;
}
ldp->min_theta = ldp->theta[0];
ldp->max_theta = ldp->theta[n-1];
//printf("LDP: theta[0] = %f, theta[n] = %f\n",ldp->min_theta, ldp->max_theta);
}
ldp->odometry[0] = 0.0;
ldp->odometry[1] = 0.0;
ldp->odometry[2] = 0.0;
ldp->true_pose[0] = 0.0;
ldp->true_pose[1] = 0.0;
ldp->true_pose[2] = 0.0;
if(n > 0)
return true;
else
return false;
}
void CanonicalScan::laserScanToLDP(const sensor_msgs::LaserScan &scan_msg, LDP &ldp)
{
unsigned int n = scan_msg.ranges.size();
ldp = ld_alloc_new(n);
for (unsigned int i = 0; i < n; i++)
{
// calculate position in laser frame
double r = scan_msg.ranges[i];
if (r > scan_msg.range_min && r < scan_msg.range_max)
{
// fill in laser scan data
ldp->valid[i] = 1;
ldp->readings[i] = r;
}
else
{
ldp->valid[i] = 0;
ldp->readings[i] = -1; // for invalid range
}
ldp->theta[i] = scan_msg.angle_min + i * scan_msg.angle_increment;
ldp->cluster[i] = -1;
}
ldp->min_theta = ldp->theta[0];
ldp->max_theta = ldp->theta[n-1];
ldp->odometry[0] = 0.0;
ldp->odometry[1] = 0.0;
ldp->odometry[2] = 0.0;
ldp->true_pose[0] = 0.0;
ldp->true_pose[1] = 0.0;
ldp->true_pose[2] = 0.0;
}
void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud,
LDP& ldp)
{
double max_d2 = cloud_res_ * cloud_res_;
PointCloudT cloud_f;
cloud_f.points.push_back(cloud->points[0]);
for (unsigned int i = 1; i < cloud->points.size(); ++i)
{
const PointT& pa = cloud_f.points[cloud_f.points.size() - 1];
const PointT& pb = cloud->points[i];
double dx = pa.x - pb.x;
double dy = pa.y - pb.y;
double d2 = dx*dx + dy*dy;
if (d2 > max_d2)
{
cloud_f.points.push_back(pb);
}
}
unsigned int n = cloud_f.points.size();
ldp = ld_alloc_new(n);
for (unsigned int i = 0; i < n; i++)
{
// calculate position in laser frame
if (is_nan(cloud_f.points[i].x) || is_nan(cloud_f.points[i].y))
{
ROS_WARN("Laser Scan Matcher: Cloud input contains NaN values. \
Please use a filtered cloud input.");
}
else
{
LDP ld_from_carmen_string(const char*line) {
if(0 != strncmp(line, carmen_prefix, strlen(carmen_prefix))) {
sm_error("This is not a Carmen line: \n-> %s\n", line);
return 0;
}
size_t cur = strlen(carmen_prefix);
int nrays=-1;
if(read_next_integer(line, &cur, &nrays)) {
sm_error("Could not get number of rays.\n");
goto error;
}
LDP ld = ld_alloc_new(nrays);
double fov = M_PI;
double min_reading = 0;
double max_reading = 80;
if(nrays == 769) {
min_reading = 0.001;
max_reading = 4;
fov = deg2rad(270.0);
static int print = 0;
if(!print) { print = 1;
sm_info("Assuming that 769 rays is an Hokuyo "
"with fov = %f deg, min_reading = %f m, max_reading = %fm\n",
rad2deg(fov), min_reading, max_reading);
}
}
ld->min_theta = -fov/2;
ld->max_theta = +fov/2;
int on_error = 0;
int i;
for(i=0;i<nrays;i++) {
double reading;
if(read_next_double(line,&cur,&reading)) {
sm_error("Could not read ray #%d / %d, \n", i, nrays);
on_error = 1;
break;
}
ld->valid[i] = (reading > min_reading) && (reading < max_reading);
ld->readings[i] = ld->valid[i] ? reading : NAN;
ld->theta[i] = ld->min_theta + i *
(ld->max_theta-ld->min_theta) / (ld->nrays-1);
/* bad hokuyo!! */
if(nrays == 769) {
if(i>725 || i<44) {
ld->valid[i] = 0;
ld->readings[i] = NAN;
}
}
}
if(on_error) goto error;
if(read_next_double(line,&cur,ld->estimate+0)) goto error;
if(read_next_double(line,&cur,ld->estimate+1)) goto error;
if(read_next_double(line,&cur,ld->estimate+2)) goto error;
if(read_next_double(line,&cur,ld->odometry+0)) goto error;
if(read_next_double(line,&cur,ld->odometry+1)) goto error;
if(read_next_double(line,&cur,ld->odometry+2)) goto error;
/* Following: ipc_timestamp hostname timestamp */
/* Two csm_options:
double string double:
the first is timestamp in seconds, the second is discarded
int string int:
the first is sec, the second is usec
*/
static int warn_format = 1;
int inc; int sec=-1, usec=-1;
int res = sscanf(line + cur, "%d %s %d%n", &sec, ld->hostname, &usec, &inc);
if(3 == res) {
ld->tv.tv_sec = sec;
ld->tv.tv_usec = usec;
if(warn_format)
sm_info("Reading timestamp as 'sec hostname usec'.\n");
} else {
double v1=-1, v2=-1;
res = sscanf(line + cur, "%lf %s %lf%n", &v1, ld->hostname, &v2, &inc);
if(3 == res) {
ld->tv.tv_sec = (int) floor(v1);
ld->tv.tv_usec = floor( (v1 - floor(v1)) * 1e6 );
if(warn_format)
sm_info("Reading timestamp as doubles (discarding second one).\n");
} else {
ld->tv.tv_sec = 0;
ld->tv.tv_usec = 0;
if(warn_format)
sm_info("I could not read timestamp+hostname; ignoring (I will warn only once for this).\n");
}
}
warn_format = 0;
fprintf(stderr, "l");
return ld;
error:
printf("Malformed line: '%s'\nat cur = %d\n\t-> '%s'\n", line,(int)cur,line+cur);
return 0;
}
