Skip to content

Commit

Permalink
Everything builds with catkin. A bit more cleanup is needed.
Browse files Browse the repository at this point in the history
  • Loading branch information
chadrockey committed Mar 27, 2013
1 parent 178cebd commit ec34bf7
Show file tree
Hide file tree
Showing 14 changed files with 158 additions and 85 deletions.
90 changes: 85 additions & 5 deletions CMakeLists.txt
@@ -1,5 +1,85 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
subdirs(standalone ros)
cmake_minimum_required(VERSION 2.8.3)
project(sicktoolbox_wrapper)

find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs sicktoolbox rosconsole diagnostic_updater)

## System dependencies are found with CMake's conventions
#find_package(Threads)

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS
LIBRARIES
CATKIN_DEPENDS roscpp sensor_msgs sicktoolbox rosconsole diagnostic_updater
DEPENDS
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS})

add_executable(sickld ros/sickld/sickld.cpp)
target_link_libraries(sickld ${catkin_LIBRARIES})

add_executable(sicklms ros/sicklms/sicklms.cpp)
target_link_libraries(sicklms ${catkin_LIBRARIES})

add_executable(print_scans standalone/print_scans.cpp)
target_link_libraries(print_scans ${catkin_LIBRARIES})

add_executable(time_scans standalone/time_scans.cpp)
target_link_libraries(time_scans ${catkin_LIBRARIES})

add_executable(log_scans standalone/log_scans.cpp)
target_link_libraries(log_scans ${catkin_LIBRARIES})

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
#install(TARGETS SickLD SickLMS1xx SickLMS2xx LDConfigLib ld_config
# ld_more_config ld_multi_sector ld_single_sector lms1xx_simple_app
# lms2xx_config lms2xx_mean_values lms2xx_partial_scan lms2xx_real_time_indices
# lms2xx_set_variant lms2xx_simple_app lms2xx_stream_range_and_reflect lms2xx_subrange
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
#)

## TODO Move headers that aren't needed externally back into the source
## Mark cpp header files for installation
#install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.hh"
# PATTERN ".svn" EXCLUDE
#)

## TODO There are readmes, etc, create install rules for these
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
1 change: 0 additions & 1 deletion Makefile

This file was deleted.

9 changes: 0 additions & 9 deletions mainpage.dox

This file was deleted.

17 changes: 0 additions & 17 deletions manifest.xml

This file was deleted.

35 changes: 35 additions & 0 deletions package.xml
@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<package>
<name>sicktoolbox_wrapper</name>
<version>2.5.0</version>
<description>sicktoolbox_wrapper is a ROS wrapper for the outstanding <a href="http://www.ros.org/wiki/sicktoolbox">sicktoolbox</a>
library for interfacing with the SICK LMS2xx lasers.
</description>

<maintainer email="chadrockey@willowgarage.com">Chad Rockey</maintainer>

<license>BSD</license>

<url type="website">http://ros.org/wiki/sicktoolbox_wrapper</url>
<url type="bugtracker">https://github.com/ros-drivers/sicktoolbox_wrapper/issues</url>
<url type="repository">https://github.com/ros-drivers/sicktoolbox_wrapper</url>

<author email="mquigley@cs.stanford.edu">Morgan Quigley</author>

<buildtool_depend>catkin</buildtool_depend>

<build_depend>roscpp</build_depend>
<build_depend>rosconsole</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>sicktoolbox</build_depend>
<build_depend>diagnostic_updater</build_depend>

<run_depend>roscpp</run_depend>
<run_depend>rosconsole</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>sicktoolbox</run_depend>
<run_depend>diagnostic_updater</run_depend>

<export>
</export>
</package>
2 changes: 0 additions & 2 deletions ros/CMakeLists.txt

This file was deleted.

2 changes: 0 additions & 2 deletions ros/sickld/CMakeLists.txt

This file was deleted.

2 changes: 1 addition & 1 deletion ros/sickld/sickld.cpp
Expand Up @@ -8,7 +8,7 @@
*/

#include <iostream>
#include <sickld-1.0/SickLD.hh>
#include <sicktoolbox/SickLD.hh>
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include <deque>
Expand Down
3 changes: 0 additions & 3 deletions ros/sicklms/CMakeLists.txt

This file was deleted.

44 changes: 22 additions & 22 deletions ros/sicklms/sicklms.cpp
Expand Up @@ -33,7 +33,7 @@
#include <cstdio>
#include <math.h>
#include <limits>
#include <sicklms-1.0/SickLMS.hh>
#include <sicktoolbox/SickLMS2xx.hh>
#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include <diagnostic_updater/diagnostic_updater.h> // Publishing over the diagnostics channels.
Expand Down Expand Up @@ -154,14 +154,14 @@ void publish_scan(diagnostic_updater::DiagnosedPublisher<sensor_msgs::LaserScan>
pub->publish(scan_msg);
}

SickLMS::sick_lms_measuring_units_t StringToLmsMeasuringUnits(string units)
SickLMS2xx::sick_lms_2xx_measuring_units_t StringToLmsMeasuringUnits(string units)
{
if (units.compare("mm") == 0)
return SickLMS::SICK_MEASURING_UNITS_MM;
return SickLMS2xx::SICK_MEASURING_UNITS_MM;
else if (units.compare("cm") == 0)
return SickLMS::SICK_MEASURING_UNITS_CM;
return SickLMS2xx::SICK_MEASURING_UNITS_CM;

return SickLMS::SICK_MEASURING_UNITS_UNKNOWN;
return SickLMS2xx::SICK_MEASURING_UNITS_UNKNOWN;
}


Expand Down Expand Up @@ -231,17 +231,17 @@ int main(int argc, char **argv)
ROS_WARN("The use_rep_117 parameter is set to false. This parameter will be removed in Hydromedusa. Please see: http://ros.org/wiki/rep_117/migration");
}

SickLMS::sick_lms_baud_t desired_baud = SickLMS::IntToSickBaud(baud);
if (desired_baud == SickLMS::SICK_BAUD_UNKNOWN)
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::IntToSickBaud(baud);
if (desired_baud == SickLMS2xx::SICK_BAUD_UNKNOWN)
{
ROS_ERROR("Baud rate must be in {9600, 19200, 38400, 500000}");
return 1;
}
uint32_t range_values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t intensity_values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t range_values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t intensity_values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t n_range_values = 0;
uint32_t n_intensity_values = 0;
SickLMS sick_lms(port);
SickLMS2xx sick_lms(port);
double scale = 0;
double angle_offset;
uint32_t partial_scan_index;
Expand All @@ -258,7 +258,7 @@ int main(int argc, char **argv)
// the user specifies a setting.
int actual_angle = sick_lms.GetSickScanAngle();
double actual_resolution = sick_lms.GetSickScanResolution();
SickLMS::sick_lms_measuring_units_t actual_units = sick_lms.GetSickMeasuringUnits();
SickLMS2xx::sick_lms_2xx_measuring_units_t actual_units = sick_lms.GetSickMeasuringUnits();

// Attempt to set measurement angles and angular resolution
try {
Expand Down Expand Up @@ -304,9 +304,9 @@ int main(int argc, char **argv)
}
}

if (actual_units == SickLMS::SICK_MEASURING_UNITS_CM)
if (actual_units == SickLMS2xx::SICK_MEASURING_UNITS_CM)
scale = 0.01;
else if (actual_units == SickLMS::SICK_MEASURING_UNITS_MM)
else if (actual_units == SickLMS2xx::SICK_MEASURING_UNITS_MM)
scale = 0.001;
else
{
Expand All @@ -318,29 +318,29 @@ int main(int argc, char **argv)
// for the mirror to rotate. If we have a higher resolution, the
// SICKs interleave the readings, so the net result is we just
// shift the measurements.
if (angle == 180 || sick_lms.IsSickLMSFast()) {
if (angle == 180 || sick_lms.IsSickLMS2xxFast()) {
scan_time = 1.0 / 75;
}
else {
SickLMS::sick_lms_scan_resolution_t scan_resolution =
SickLMS::DoubleToSickScanResolution(resolution);
if ( scan_resolution == SickLMS::SICK_SCAN_RESOLUTION_25) {
SickLMS2xx::sick_lms_2xx_scan_resolution_t scan_resolution =
SickLMS2xx::DoubleToSickScanResolution(resolution);
if ( scan_resolution == SickLMS2xx::SICK_SCAN_RESOLUTION_25) {
// 0.25 degrees
scan_time = 4.0 / 75; // 53.33 ms
}
else if ( scan_resolution == SickLMS::SICK_SCAN_RESOLUTION_50) {
else if ( scan_resolution == SickLMS2xx::SICK_SCAN_RESOLUTION_50) {
// 0.5 degrees
scan_time = 2.0 / 75; // 26.66 ms
}
else if ( scan_resolution == SickLMS::SICK_SCAN_RESOLUTION_100) {
else if ( scan_resolution == SickLMS2xx::SICK_SCAN_RESOLUTION_100) {
// 1 degree
scan_time = 1.0 / 75; // 13.33 ms
}
else {
ROS_ERROR("Bogus scan resolution.");
return 1;
}
if ( scan_resolution != SickLMS::SICK_SCAN_RESOLUTION_100) {
if ( scan_resolution != SickLMS2xx::SICK_SCAN_RESOLUTION_100) {
ROS_WARN("You are using an angle smaller than 180 degrees and a "
"scan resolution less than 1 degree per scan. Thus, "
"you are in inteleaved mode and the returns will not "
Expand All @@ -356,7 +356,7 @@ int main(int argc, char **argv)
// 0.5 or 0.25 degrees resolution because it is interleaved. So for
// 0.5 degrees, two scans are needed, offset by 0.5 degrees. These
// show up as two seperate LaserScan messages.
angle_increment = sick_lms.IsSickLMSFast() ? 0.5 : 1.0;
angle_increment = sick_lms.IsSickLMS2xxFast() ? 0.5 : 1.0;

angle_offset = (180.0-angle)/2;
}
Expand All @@ -369,7 +369,7 @@ int main(int argc, char **argv)
{
while (ros::ok())
{
if (sick_lms.IsSickLMSFast()) {
if (sick_lms.IsSickLMS2xxFast()) {
// There's no inteleaving, but we can capture both the range
// and intensity simultaneously
sick_lms.GetSickScan(range_values, intensity_values,
Expand Down
8 changes: 0 additions & 8 deletions standalone/CMakeLists.txt

This file was deleted.

10 changes: 5 additions & 5 deletions standalone/log_scans.cpp
Expand Up @@ -34,7 +34,7 @@ extern "C" {
#include <stdint.h>
}
#include <cstdio>
#include <sicklms-1.0/SickLMS.hh>
#include <sicktoolbox/SickLMS2xx.hh>
#include <ros/time.h>
using namespace SickToolbox;
using namespace std;
Expand All @@ -59,16 +59,16 @@ int main(int argc, char **argv)
return 1;
}
string lms_dev = argv[1];
SickLMS::sick_lms_baud_t desired_baud = SickLMS::StringToSickBaud(argv[2]);
if (desired_baud == SickLMS::SICK_BAUD_UNKNOWN)
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::StringToSickBaud(argv[2]);
if (desired_baud == SickLMS2xx::SICK_BAUD_UNKNOWN)
{
printf("bad baud rate. must be one of {9600, 19200, 38400, 500000}\n");
return 1;
}
signal(SIGINT, ctrlc_handler);
uint32_t values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t num_values = 0;
SickLMS sick_lms(lms_dev);
SickLMS2xx sick_lms(lms_dev);
try
{
sick_lms.Initialize(desired_baud);
Expand Down
10 changes: 5 additions & 5 deletions standalone/print_scans.cpp
Expand Up @@ -36,7 +36,7 @@ extern "C" {
#include <stdint.h>
}
#include <cstdio>
#include <sicklms-1.0/SickLMS.hh>
#include <sicktoolbox/SickLMS2xx.hh>
using namespace SickToolbox;
using namespace std;

Expand All @@ -54,16 +54,16 @@ int main(int argc, char **argv)
return 1;
}
string lms_dev = argv[1];
SickLMS::sick_lms_baud_t desired_baud = SickLMS::StringToSickBaud(argv[2]);
if (desired_baud == SickLMS::SICK_BAUD_UNKNOWN)
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::StringToSickBaud(argv[2]);
if (desired_baud == SickLMS2xx::SICK_BAUD_UNKNOWN)
{
printf("bad baud rate. must be one of {9600, 19200, 38400, 500000}\n");
return 1;
}
signal(SIGINT, ctrlc_handler);
uint32_t values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0};
uint32_t num_values = 0;
SickLMS sick_lms(lms_dev);
SickLMS2xx sick_lms(lms_dev);
try
{
sick_lms.Initialize(desired_baud);
Expand Down

0 comments on commit ec34bf7

Please sign in to comment.