Open reed-adept opened 8 years ago
Looks like I need to fix this issue since the Pioneer LX has LaserIgnore set. Seems that the S300 range of vision is wider than the aperture, so the robot sees its own body at the extrema of the range. ROS thinks this is an obstacle, of course.
The easy way is to just remove that data from the message and adjust the min and max angles accordingly. This won't work if LaserIgnore partitions the range of vision into two parts, however, since ROS LaserScan messages are supposed to be contiguous. Don't know if there are any MobileRobots platforms where this is the case.
What does ROS use for a sample that doesn’t hit any obstacle (max range?) That’s what I would use. ARIA uses this when creating its point cloud data.
There are other robots where there are structural supports within the laser field of view, and we want customers to have the ability to modify or add on to their robot requiring such supports, which is the main reason for LaserIgnore parameter.
From: Blake Anderson [mailto:notifications@github.com] Sent: Tuesday, July 26, 2016 6:35 PM To: MobileRobots/ros-arnl Cc: Reed Hedges; Author Subject: Re: [MobileRobots/ros-arnl] check LaserIgnore flag when publishing laserscan? (#13)
Looks like I need to fix this issue since the Pioneer LX has LaserIgnore set. Seems that the S300 range of vision is wider than the aperture, so the robot sees its own body at the extrema of the range. ROS thinks this is an obstacle, of course.
The easy way is to just remove that data from the message and adjust the min and max angles accordingly. This won't work if LaserIgnore partitions the range of vision into two parts, however, since ROS LaserScan messages are supposed to be contiguous. Don't know if there are any MobileRobots platforms where this is the case.
— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHubhttps://github.com/MobileRobots/ros-arnl/issues/13#issuecomment-235427115, or mute the threadhttps://github.com/notifications/unsubscribe-auth/AC4MAZZs1S3a2ZmX5kJhUiWCYV-_ALHCks5qZouLgaJpZM4GyMQS.
I got it working with these changes. I basically made a bitmask to specify which indices to ignore.
At the end of LaserPublisher constructor. This requires some other work on our fork that converts the .p file to ROS params.
// Check LaserIgnore setting
std::vector<int> laser_ignore_list;
_n.getParam("Laser_parameters/LaserIgnore", laser_ignore_list);
// LaserIgnore is assumed to consist of ranges given by pairs of values
if (laser_ignore_list.size() % 2) {
ROS_WARN("Parameter LaserIgnore has odd number of values: %u", laser_ignore_list.size());
}
else {
//// Determine indices to ignore in laser data
// Iterate over range of vision
for (float angle = laser->getStartDegrees(), end = laser->getEndDegrees(); angle <= end; angle += laser->getIncrementChoiceDouble()) {
laser_ignore_indices.push_back(true); // Data is valid by default
// Check each ignore range
for (size_t i = 0; i < laser_ignore_list.size(); i += 2) {
// Check if angle is in ignore range
if (angle >= laser_ignore_list.at(i) && angle <= laser_ignore_list.at(i+1)) {
laser_ignore_indices.back() = false; // Data is not valid
break;
}
}
}
}
And:
void LaserPublisher::publishLaserScan()
{
laserscan.header.stamp = convertArTimeToROS(laser->getLastReadingTime());
const std::list<ArSensorReading*> *readings = laser->getRawReadings();
assert(readings);
laserscan.ranges.resize(readings->size());
laser_ignore_indices.resize(readings->size());
size_t n = 0;
if (laser->getFlipped()) {
// Reverse the data
for(std::list<ArSensorReading*>::const_reverse_iterator r = readings->rbegin(); r != readings->rend(); ++r)
{
assert(*r);
if (laser_ignore_indices[n]) {
laserscan.ranges[n] = (*r)->getRange() / 1000.0;
}
else {
laserscan.ranges[n] = -1;
}
++n;
}
}
else {
for(std::list<ArSensorReading*>::const_iterator r = readings->begin(); r != readings->end(); ++r)
{
assert(*r);
if (laser_ignore_indices[n]) {
laserscan.ranges[n] = (*r)->getRange() / 1000.0;
}
else {
laserscan.ranges[n] = -1;
}
++n;
}
}
laserscan_pub.publish(laserscan);
}
You should be able to just get the ignore flag from the ArSensorReading structs, you don’t need to look at the LaserIgnore parameter separately. It might be better to keep those separate. In publishLaserScan() when iterating over the ArSensorReading pointers, just check (*r)->getIgnoreThisReading(). We can also check whether its within the configured field of view here as well.
The other option for dealing with this stuff is to use ROS Laser Filter nodes. This is the more ROS-like way to deal with this. I.e. if you were using the ROS driver node for a laser, then it would just give you the laser data without these conditions. You would then use the ROS laser filter nodes to flip, filter, etc.
From: Blake Anderson [mailto:notifications@github.com] Sent: Wednesday, July 27, 2016 2:34 PM To: MobileRobots/ros-arnl Cc: Reed Hedges; Author Subject: Re: [MobileRobots/ros-arnl] check LaserIgnore flag when publishing laserscan? (#13)
I got it working with these changes. I basically made a bitmask to specify which indices to ignore.
At the end of LaserPublisher constructor. This requires some other work on our fork that converts the .p file to ROS params.
// Check LaserIgnore setting
std::vector
_n.getParam("Laser_parameters/LaserIgnore", laser_ignore_list);
// LaserIgnore is assumed to consist of ranges given by pairs of values
if (laser_ignore_list.size() % 2) {
ROS_WARN("Parameter LaserIgnore has odd number of values: %u", laser_ignore_list.size());
}
else {
//// Determine indices to ignore in laser data
// Iterate over range of vision
for (float angle = laser->getStartDegrees(), end = laser->getEndDegrees(); angle <= end; angle += laser->getIncrementChoiceDouble()) {
laser_ignore_indices.push_back(true); // Data is valid by default
// Check each ignore range
for (size_t i = 0; i < laser_ignore_list.size(); i += 2) {
// Check if angle is in ignore range
if (angle >= laser_ignore_list.at(i) && angle <= laser_ignore_list.at(i+1)) {
laser_ignore_indices.back() = false; // Data is not valid
break;
}
}
}
}
And:
void LaserPublisher::publishLaserScan()
{
laserscan.header.stamp = convertArTimeToROS(laser->getLastReadingTime());
const std::list<ArSensorReading> readings = laser->getRawReadings();
assert(readings);
laserscan.ranges.resize(readings->size());
laser_ignore_indices.resize(readings->size());
size_t n = 0;
if (laser->getFlipped()) {
// Reverse the data
for(std::list<ArSensorReading*>::const_reverse_iterator r = readings->rbegin(); r != readings->rend(); ++r)
{
assert(*r);
if (laser_ignore_indices[n]) {
laserscan.ranges[n] = (*r)->getRange() / 1000.0;
}
else {
laserscan.ranges[n] = -1;
}
++n;
}
}
else {
for(std::list<ArSensorReading*>::const_iterator r = readings->begin(); r != readings->end(); ++r)
{
assert(*r);
if (laser_ignore_indices[n]) {
laserscan.ranges[n] = (*r)->getRange() / 1000.0;
}
else {
laserscan.ranges[n] = -1;
}
++n;
}
}
laserscan_pub.publish(laserscan);
}
— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHubhttps://github.com/MobileRobots/ros-arnl/issues/13#issuecomment-235678078, or mute the threadhttps://github.com/notifications/unsubscribe-auth/AC4MAeRAVfoliFNgauDao7Rg8okGwJ3Dks5qZ6ShgaJpZM4GyMQS.
I just made a pull request #19 fixing this issue using your suggested method.
or perhaps explain how to use laser_filters to do it?