diff --git a/CMakeLists.txt b/CMakeLists.txt
index c8dedc7..153b6a7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,18 +5,21 @@ project(lms1xx)
find_package(console_bridge REQUIRED)
include_directories(include ${console_bridge_INCLUDE_DIRS})
add_library(LMS1xx src/LMS1xx.cpp)
+set_property(TARGET LMS1xx PROPERTY POSITION_INDEPENDENT_CODE ON)
target_link_libraries(LMS1xx ${console_bridge_LIBRARIES})
# Regular catkin package follows.
-find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs)
-catkin_package(CATKIN_DEPENDS roscpp)
+find_package(catkin REQUIRED COMPONENTS nodelet roscpp sensor_msgs)
+catkin_package(CATKIN_DEPENDS nodelet roscpp)
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(LMS1xx_node src/LMS1xx_node.cpp)
target_link_libraries(LMS1xx_node LMS1xx ${catkin_LIBRARIES})
+add_library(LMS1xx_nodelet src/LMS1xx_nodelet.cpp)
+target_link_libraries(LMS1xx_nodelet LMS1xx ${catkin_LIBRARIES})
-install(TARGETS LMS1xx LMS1xx_node
+install(TARGETS LMS1xx LMS1xx_node LMS1xx_nodelet
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -25,6 +28,9 @@ install(TARGETS LMS1xx LMS1xx_node
install(DIRECTORY meshes launch urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(FILES nodelets.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
install(PROGRAMS scripts/find_sick scripts/set_sick_ip
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/README.md b/README.md
index ec2c167..7d07ac6 100644
--- a/README.md
+++ b/README.md
@@ -2,3 +2,10 @@ LMS1xx [.
+
+
+Docs
+====
+
+* [Technical Information](https://cdn.sick.com/media/docs/7/27/927/Technical_information_Telegram_Listing_Ranging_sensors_LMS1xx_LMS5xx_TiM5xx_TiM7xx_LMS1000_MRS1000_MRS6000_NAV310_LD_OEM15xx_LD_LRS36xx_LMS4000_en_IM0045927.PDF)
+* [Operating Instructions](https://cdn.sick.com/media/docs/1/31/331/Operating_instructions_LMS1xx_Laser_Measurement_Sensors_en_IM0031331.PDF)
\ No newline at end of file
diff --git a/launch/LMS1xx.launch b/launch/LMS1xx.launch
index ea81081..3c6b51e 100644
--- a/launch/LMS1xx.launch
+++ b/launch/LMS1xx.launch
@@ -1,8 +1,23 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/nodelets.xml b/nodelets.xml
new file mode 100644
index 0000000..49b504d
--- /dev/null
+++ b/nodelets.xml
@@ -0,0 +1,7 @@
+
+
+
+ Nodelet version of LMS1xx driver.
+
+
+
\ No newline at end of file
diff --git a/package.xml b/package.xml
index d68a190..748477b 100644
--- a/package.xml
+++ b/package.xml
@@ -11,6 +11,7 @@
LGPL
catkin
+ nodelet
rosconsole_bridge
roscpp
roscpp_serialization
@@ -19,4 +20,8 @@
roslaunch
roslint
rosunit
+
+
+
+
diff --git a/src/LMS1xx.cpp b/src/LMS1xx.cpp
index f61d45d..ea7cf41 100644
--- a/src/LMS1xx.cpp
+++ b/src/LMS1xx.cpp
@@ -363,6 +363,27 @@ void LMS1xx::parseScanData(char* buffer, scanData* data)
data->rssi_len2 = NumberData;
}
+
+ /*
+ * TODO: there are range/rssi values with special meaning!
+ *
+ * LMDscandata - reserved valuesValid distance measurement values are values starting from 16d upwards;
+ * everything below has the following meaning:
+ *
+ * DIST RSSI Description
+ * 0d 0h no meas value detected; means that in the angle, there was no valid measurement
+ * value. Probably the object to measure was out of the range of the or the object
+ * was reflecting too less light back (black objects)
+ * 1d FFFFh (16Bit output)
+ * FFh(8Bit output)
+ * dazzled, blinded
+ * 2d 0h implausible measurement values
+ * 3d 0h value was set to invalid by a filter (Echo Filter,Particle Filterin old firmware)
+ * 4–15d 0h reserved, at the moment not given out, if there occurs a value in that range any
+ * way perform a Softwareupdate
+ * ≥16d >0h valid measurement val
+ */
+
for (int i = 0; i < NumberData; i++)
{
int dat;
diff --git a/src/LMS1xx_node.cpp b/src/LMS1xx_node.cpp
index 03fbf28..3a00b40 100644
--- a/src/LMS1xx_node.cpp
+++ b/src/LMS1xx_node.cpp
@@ -38,22 +38,28 @@ int main(int argc, char **argv)
scanCfg cfg;
scanOutputRange outputRange;
scanDataCfg dataCfg;
- sensor_msgs::LaserScan scan_msg;
+ sensor_msgs::LaserScan scan_msg, scan2_msg;
// parameters
std::string host;
std::string frame_id;
bool inf_range;
+ bool publish_2nd_pulse;
int port;
+ float range_min, range_max;
ros::init(argc, argv, "lms1xx");
ros::NodeHandle nh;
ros::NodeHandle n("~");
ros::Publisher scan_pub = nh.advertise("scan", 1);
+ ros::Publisher scan2_pub = nh.advertise("scan_2nd_pulse", 1);
n.param("host", host, "192.168.1.2");
n.param("frame_id", frame_id, "laser");
n.param("publish_min_range_as_inf", inf_range, false);
+ n.param("publish_2nd_pulse", publish_2nd_pulse, false);
+ n.param("range_min", range_min, 0.01);
+ n.param("range_max", range_max, 20.0);
n.param("port", port, 2111);
while (ros::ok())
@@ -87,14 +93,16 @@ int main(int argc, char **argv)
ROS_DEBUG("Laser output range:angleResolution %d, startAngle %d, stopAngle %d",
outputRange.angleResolution, outputRange.startAngle, outputRange.stopAngle);
- scan_msg.header.frame_id = frame_id;
- scan_msg.range_min = 0.01;
- scan_msg.range_max = 20.0;
- scan_msg.scan_time = 100.0 / cfg.scaningFrequency;
- scan_msg.angle_increment = static_cast(outputRange.angleResolution / 10000.0 * DEG2RAD);
- scan_msg.angle_min = static_cast(outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2);
- scan_msg.angle_max = static_cast(outputRange.stopAngle / 10000.0 * DEG2RAD - M_PI / 2);
-
+ scan_msg.header.frame_id = scan2_msg.header.frame_id = frame_id;
+ scan_msg.range_min = scan2_msg.range_min = range_min;
+ scan_msg.range_max = scan2_msg.range_max = range_max;
+ scan_msg.scan_time = scan2_msg.scan_time = 100.0 / cfg.scaningFrequency;
+ scan_msg.angle_increment = scan2_msg.angle_increment =
+ static_cast(outputRange.angleResolution / 10000.0 * DEG2RAD);
+ scan_msg.angle_min = scan2_msg.angle_min = static_cast(
+ outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2);
+ scan_msg.angle_max = scan2_msg.angle_max = static_cast(
+ outputRange.stopAngle / 10000.0 * DEG2RAD - M_PI / 2);
ROS_DEBUG_STREAM("Device resolution is " << (double)outputRange.angleResolution / 10000.0 << " degrees.");
ROS_DEBUG_STREAM("Device frequency is " << (double)cfg.scaningFrequency / 100.0 << " Hz");
@@ -107,15 +115,19 @@ int main(int argc, char **argv)
}
scan_msg.ranges.resize(num_values);
scan_msg.intensities.resize(num_values);
+ if (publish_2nd_pulse) {
+ scan2_msg.ranges.resize(num_values);
+ scan2_msg.intensities.resize(num_values);
+ }
- scan_msg.time_increment =
+ scan_msg.time_increment = scan2_msg.time_increment =
(outputRange.angleResolution / 10000.0)
/ 360.0
/ (cfg.scaningFrequency / 100.0);
ROS_DEBUG_STREAM("Time increment is " << static_cast(scan_msg.time_increment * 1000000) << " microseconds");
- dataCfg.outputChannel = 1;
+ dataCfg.outputChannel = publish_2nd_pulse ? 3 : 1;
dataCfg.remission = true;
dataCfg.resolution = 1;
dataCfg.encoder = 0;
@@ -173,8 +185,9 @@ int main(int argc, char **argv)
{
ros::Time start = ros::Time::now();
- scan_msg.header.stamp = start;
+ scan_msg.header.stamp = scan2_msg.header.stamp = start;
++scan_msg.header.seq;
+ ++scan2_msg.header.seq;
scanData data;
ROS_DEBUG("Reading scan data.");
@@ -182,7 +195,7 @@ int main(int argc, char **argv)
{
for (int i = 0; i < data.dist_len1; i++)
{
- float range_data = data.dist1[i] * 0.001;
+ float range_data = data.dist1[i] * 0.001f;
if (inf_range && range_data < scan_msg.range_min)
{
@@ -201,6 +214,25 @@ int main(int argc, char **argv)
ROS_DEBUG("Publishing scan data.");
scan_pub.publish(scan_msg);
+
+ if (publish_2nd_pulse) {
+ for (int i = 0; i < data.dist_len2; i++) {
+ float range_data = data.dist2[i] * 0.001f;
+
+ if (inf_range && range_data < scan2_msg.range_min) {
+ scan2_msg.ranges[i] = std::numeric_limits::infinity();
+ } else {
+ scan2_msg.ranges[i] = range_data;
+ }
+ }
+
+ for (int i = 0; i < data.rssi_len2; i++) {
+ scan2_msg.intensities[i] = data.rssi2[i];
+ }
+
+ ROS_DEBUG("Publishing second pulse scan data.");
+ scan2_pub.publish(scan2_msg);
+ }
}
else
{
diff --git a/src/LMS1xx_nodelet.cpp b/src/LMS1xx_nodelet.cpp
new file mode 100644
index 0000000..6ac241b
--- /dev/null
+++ b/src/LMS1xx_nodelet.cpp
@@ -0,0 +1,263 @@
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+#define DEG2RAD M_PI/180.0
+
+namespace lms1xx {
+
+class LMS1xxNodelet : public nodelet::Nodelet {
+
+public:
+ virtual ~LMS1xxNodelet();
+
+protected:
+ void onInit() override;
+ void loop();
+
+ std::thread thread;
+
+ LMS1xx laser;
+ scanCfg cfg;
+ scanOutputRange outputRange;
+ scanDataCfg dataCfg;
+ ros::Publisher scan_pub, scan2_pub;
+
+ // parameters
+ std::string host;
+ std::string frame_id;
+ bool inf_range;
+ bool publish_2nd_pulse;
+ int port;
+ float range_min, range_max;
+
+ ros::NodeHandle nh, n;
+
+ volatile bool isShuttingDown = false;
+
+ bool ok() const {
+ return !isShuttingDown && ros::ok();
+ }
+};
+
+
+void LMS1xxNodelet::onInit() {
+ nh = getNodeHandle();
+ n = getPrivateNodeHandle();
+
+ n.param("host", host, "192.168.1.2");
+ n.param("frame_id", frame_id, "laser");
+ n.param("publish_min_range_as_inf", inf_range, false);
+ n.param("publish_2nd_pulse", publish_2nd_pulse, false);
+ n.param("range_min", range_min, 0.01);
+ n.param("range_max", range_max, 20.0);
+ n.param("port", port, 2111);
+
+ if (publish_2nd_pulse)
+ NODELET_DEBUG("Reading and publishing also second laser reflections.");
+
+ scan_pub = nh.advertise("scan", 1);
+ if (publish_2nd_pulse)
+ scan2_pub = nh.advertise("scan_2nd_pulse", 1);
+
+ thread = std::thread(&LMS1xxNodelet::loop, this);
+}
+
+void LMS1xxNodelet::loop() {
+ while (this->ok()) {
+ NODELET_INFO_STREAM("Connecting to laser at " << host);
+ laser.connect(host, port);
+ if (!laser.isConnected()) {
+ NODELET_WARN("Unable to connect, retrying.");
+ ros::WallDuration(1).sleep();
+ continue;
+ }
+
+ NODELET_DEBUG("Logging in to laser.");
+ laser.login();
+ cfg = laser.getScanCfg();
+ outputRange = laser.getScanOutputRange();
+
+ if (cfg.scaningFrequency != 5000) {
+ laser.disconnect();
+ NODELET_WARN("Unable to get laser output range. Retrying.");
+ ros::WallDuration(1).sleep();
+ continue;
+ }
+
+ NODELET_INFO("Connected to laser.");
+
+ NODELET_DEBUG("Laser configuration: scaningFrequency %d, angleResolution %d, startAngle %d, stopAngle %d",
+ cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle,
+ cfg.stopAngle);
+ NODELET_DEBUG(
+ "Laser output range:angleResolution %d, startAngle %d, stopAngle %d",
+ outputRange.angleResolution, outputRange.startAngle,
+ outputRange.stopAngle);
+
+ sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan());
+ sensor_msgs::LaserScanPtr scan2_msg(new sensor_msgs::LaserScan());
+
+ scan_msg->header.frame_id = scan2_msg->header.frame_id = frame_id;
+ scan_msg->range_min = scan2_msg->range_min = range_min;
+ scan_msg->range_max = scan2_msg->range_max = range_max;
+ scan_msg->scan_time = scan2_msg->scan_time = 100.0 / cfg.scaningFrequency;
+ scan_msg->angle_increment = scan2_msg->angle_increment =
+ static_cast(outputRange.angleResolution / 10000.0 * DEG2RAD);
+ scan_msg->angle_min = scan2_msg->angle_min = static_cast(
+ outputRange.startAngle / 10000.0 * DEG2RAD - M_PI / 2);
+ scan_msg->angle_max = scan2_msg->angle_max = static_cast(
+ outputRange.stopAngle / 10000.0 * DEG2RAD - M_PI / 2);
+
+ NODELET_DEBUG_STREAM("Device resolution is "
+ << (double)outputRange.angleResolution / 10000.0
+ << " degrees.");
+ NODELET_DEBUG_STREAM("Device frequency is "
+ << (double)cfg.scaningFrequency / 100.0 << " Hz");
+
+ int angle_range = outputRange.stopAngle - outputRange.startAngle;
+ int num_values = angle_range / outputRange.angleResolution;
+ if (angle_range % outputRange.angleResolution == 0) {
+ // Include endpoint
+ ++num_values;
+ }
+ scan_msg->ranges.resize(num_values);
+ scan_msg->intensities.resize(num_values);
+ if (publish_2nd_pulse) {
+ scan2_msg->ranges.resize(num_values);
+ scan2_msg->intensities.resize(num_values);
+ }
+
+ scan_msg->time_increment = scan2_msg->time_increment =
+ (outputRange.angleResolution / 10000.0) / 360.0 /
+ (cfg.scaningFrequency / 100.0);
+
+ NODELET_DEBUG_STREAM("Time increment is "
+ << static_cast(scan_msg->time_increment * 1000000)
+ << " microseconds");
+
+ dataCfg.outputChannel = publish_2nd_pulse ? 3 : 1;
+ dataCfg.remission = true;
+ dataCfg.resolution = 1;
+ dataCfg.encoder = 0;
+ dataCfg.position = false;
+ dataCfg.deviceName = false;
+ dataCfg.outputInterval = 1;
+
+ NODELET_DEBUG("Setting scan data configuration.");
+ laser.setScanDataCfg(dataCfg);
+
+ NODELET_DEBUG("Starting measurements.");
+ laser.startMeas();
+
+ NODELET_DEBUG("Waiting for ready status.");
+ ros::Time ready_status_timeout = ros::Time::now() + ros::Duration(5);
+
+ // while(1)
+ // {
+ status_t stat = laser.queryStatus();
+ ros::WallDuration(1.0).sleep();
+ if (stat != ready_for_measurement) {
+ NODELET_WARN("Laser not ready. Retrying initialization.");
+ laser.disconnect();
+ ros::WallDuration(1).sleep();
+ continue;
+ }
+ /*if (stat == ready_for_measurement)
+ {
+ NODELET_DEBUG("Ready status achieved.");
+ break;
+ }
+
+ if (ros::Time::now() > ready_status_timeout)
+ {
+ NODELET_WARN("Timed out waiting for ready status. Trying again.");
+ laser.disconnect();
+ continue;
+ }
+
+ if (!ros::ok())
+ {
+ laser.disconnect();
+ return 1;
+ }
+ }*/
+
+ NODELET_DEBUG("Starting device.");
+ laser.startDevice(); // Log out to properly re-enable system after config
+
+ NODELET_DEBUG("Commanding continuous measurements.");
+ laser.scanContinous(1);
+
+ while (this->ok()) {
+ ros::Time start = ros::Time::now();
+
+ scan_msg->header.stamp = scan2_msg->header.stamp = start;
+ ++scan_msg->header.seq;
+ ++scan2_msg->header.seq;
+
+ scanData data;
+ NODELET_DEBUG("Reading scan data.");
+ if (laser.getScanData(&data)) {
+ for (int i = 0; i < data.dist_len1; i++) {
+ float range_data = data.dist1[i] * 0.001f;
+
+ if (inf_range && range_data < scan_msg->range_min) {
+ scan_msg->ranges[i] = std::numeric_limits::infinity();
+ } else {
+ scan_msg->ranges[i] = range_data;
+ }
+ }
+
+ for (int i = 0; i < data.rssi_len1; i++) {
+ scan_msg->intensities[i] = data.rssi1[i];
+ }
+
+ NODELET_DEBUG("Publishing scan data.");
+ scan_pub.publish(scan_msg);
+
+ if (publish_2nd_pulse) {
+ for (int i = 0; i < data.dist_len2; i++) {
+ float range_data = data.dist2[i] * 0.001f;
+
+ if (inf_range && range_data < scan2_msg->range_min) {
+ scan2_msg->ranges[i] = std::numeric_limits::infinity();
+ } else {
+ scan2_msg->ranges[i] = range_data;
+ }
+ }
+
+ for (int i = 0; i < data.rssi_len2; i++) {
+ scan2_msg->intensities[i] = data.rssi2[i];
+ }
+
+ NODELET_DEBUG("Publishing second pulse scan data.");
+ scan2_pub.publish(scan2_msg);
+ }
+ } else {
+ NODELET_ERROR(
+ "Laser timed out on delivering scan, attempting to reinitialize.");
+ break;
+ }
+ }
+
+ laser.scanContinous(0);
+ laser.stopMeas();
+ laser.disconnect();
+ }
+}
+
+LMS1xxNodelet::~LMS1xxNodelet() {
+ this->isShuttingDown;
+ this->thread.join();
+}
+
+}
+
+PLUGINLIB_EXPORT_CLASS(lms1xx::LMS1xxNodelet, nodelet::Nodelet)