Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added option to publish max range readings as inf #8

Open
wants to merge 1 commit into
base: indigo-devel
Choose a base branch
from
Open

Added option to publish max range readings as inf #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions include/urg_node/urg_c_wrapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,9 @@ namespace urg_node
class URGCWrapper
{
public:
URGCWrapper(const std::string& ip_address, const int ip_port, bool& using_intensity, bool& using_multiecho);
URGCWrapper(const std::string& ip_address, const int ip_port, bool& using_intensity, bool& using_multiecho, bool &no_range_as_inf);

URGCWrapper(const int serial_baud, const std::string& serial_port, bool& using_intensity, bool& using_multiecho);
URGCWrapper(const int serial_baud, const std::string& serial_port, bool& using_intensity, bool& using_multiecho, bool &no_range_as_inf);

~URGCWrapper();

Expand Down Expand Up @@ -122,7 +122,7 @@ namespace urg_node
bool grabScan(const sensor_msgs::MultiEchoLaserScanPtr& msg);

private:
void initialize(bool& using_intensity, bool& using_multiecho);
void initialize(bool& using_intensity, bool& using_multiecho, bool &no_range_as_inf);

bool isIntensitySupported();

Expand All @@ -144,6 +144,7 @@ namespace urg_node

bool use_intensity_;
bool use_multiecho_;
bool no_range_as_inf_;
urg_measurement_type_t measurement_type_;
int first_step_;
int last_step_;
Expand Down
5 changes: 3 additions & 2 deletions src/getID.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,7 @@ main(int argc, char** argv)

bool publish_intensity = false;
bool publish_multiecho = false;
bool publish_max_range_as_inf = false;
int serial_baud = 115200;
int ip_port = 10940;
std::string ip_address = "";
Expand All @@ -125,9 +126,9 @@ main(int argc, char** argv)
// Set up the urgwidget
try{
if(ip_address != ""){
urg_.reset(new urg_node::URGCWrapper(ip_address, ip_port, publish_intensity, publish_multiecho));
urg_.reset(new urg_node::URGCWrapper(ip_address, ip_port, publish_intensity, publish_multiecho, publish_max_range_as_inf));
} else {
urg_.reset(new urg_node::URGCWrapper(serial_baud, serial_port, publish_intensity, publish_multiecho));
urg_.reset(new urg_node::URGCWrapper(serial_baud, serial_port, publish_intensity, publish_multiecho, publish_max_range_as_inf));
}
std::string device_id = urg_->getDeviceID();
if (verbose){
Expand Down
20 changes: 13 additions & 7 deletions src/urg_c_wrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@

using namespace urg_node;

URGCWrapper::URGCWrapper(const std::string& ip_address, const int ip_port, bool& using_intensity, bool& using_multiecho){
URGCWrapper::URGCWrapper(const std::string& ip_address, const int ip_port, bool& using_intensity, bool& using_multiecho, bool &no_range_as_inf){
// Store for comprehensive diagnostics
ip_address_ = ip_address;
ip_port_ = ip_port;
Expand All @@ -55,10 +55,11 @@ URGCWrapper::URGCWrapper(const std::string& ip_address, const int ip_port, bool&
throw std::runtime_error(ss.str());
}

initialize(using_intensity, using_multiecho);
initialize(using_intensity, using_multiecho, no_range_as_inf);
}

URGCWrapper::URGCWrapper(const int serial_baud, const std::string& serial_port, bool& using_intensity, bool& using_multiecho){
URGCWrapper::URGCWrapper(const int serial_baud, const std::string& serial_port, bool& using_intensity, bool& using_multiecho,
bool &no_range_as_inf){
// Store for comprehensive diagnostics
serial_baud_ = serial_baud;
serial_port_ = serial_port;
Expand All @@ -77,10 +78,10 @@ URGCWrapper::URGCWrapper(const int serial_baud, const std::string& serial_port,
throw std::runtime_error(ss.str());
}

initialize(using_intensity, using_multiecho);
initialize(using_intensity, using_multiecho, no_range_as_inf);
}

void URGCWrapper::initialize(bool& using_intensity, bool& using_multiecho){
void URGCWrapper::initialize(bool& using_intensity, bool& using_multiecho, bool &no_range_as_inf){
int urg_data_size = urg_max_data_size(&urg_);
if(urg_data_size > 5000){ // Ocassionally urg_max_data_size returns a string pointer, make sure we don't allocate too much space, the current known max is 1440 steps
urg_data_size = 5000;
Expand All @@ -106,6 +107,7 @@ void URGCWrapper::initialize(bool& using_intensity, bool& using_multiecho){
use_intensity_ = using_intensity;
use_multiecho_ = using_multiecho;

no_range_as_inf_ = no_range_as_inf;
measurement_type_ = URG_DISTANCE;
if(use_intensity_ && use_multiecho_){
measurement_type_ = URG_MULTIECHO_INTENSITY;
Expand Down Expand Up @@ -184,8 +186,12 @@ bool URGCWrapper::grabScan(const sensor_msgs::LaserScanPtr& msg){
msg->intensities[i] = intensity_[i];
}
} else {
msg->ranges[i] = std::numeric_limits<float>::quiet_NaN();
continue;
if(no_range_as_inf_){
msg->ranges[i] = std::numeric_limits<float>::infinity();
}
else{
msg->ranges[i] = std::numeric_limits<float>::quiet_NaN();
}
}
}
return true;
Expand Down
7 changes: 5 additions & 2 deletions src/urg_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,6 +211,9 @@ int main(int argc, char **argv)
bool publish_multiecho;
pnh.param<bool>("publish_multiecho", publish_multiecho, true);

bool no_range_as_inf;
pnh.param<bool>("no_range_as_inf", no_range_as_inf, false);

int error_limit;
pnh.param<int>("error_limit", error_limit, 4);

Expand All @@ -231,9 +234,9 @@ int main(int argc, char **argv)
urg_.reset(); // Clear any previous connections();
ros::Duration(1.0).sleep();
if(ip_address != ""){
urg_.reset(new urg_node::URGCWrapper(ip_address, ip_port, publish_intensity, publish_multiecho));
urg_.reset(new urg_node::URGCWrapper(ip_address, ip_port, publish_intensity, publish_multiecho, no_range_as_inf));
} else {
urg_.reset(new urg_node::URGCWrapper(serial_baud, serial_port, publish_intensity, publish_multiecho));
urg_.reset(new urg_node::URGCWrapper(serial_baud, serial_port, publish_intensity, publish_multiecho, no_range_as_inf));
}
} catch(std::runtime_error& e){
ROS_ERROR_THROTTLE(10.0, "Error connecting to Hokuyo: %s", e.what());
Expand Down