init commit

adding data_capture, Onboard-SDK-ROS, and velodyne-gps_imu

data_capture/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(data_capture)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  dji_sdk
+  geometry_msgs
+)
+
+find_package(DJIOSDK REQUIRED)
+
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## 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 include
+#  LIBRARIES data_capture
+#  CATKIN_DEPENDS roscpp rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(waypoint_mission
+        src/waypoint_mission.cpp)
+target_link_libraries(waypoint_mission
+        ${catkin_LIBRARIES}
+        ${DJIOSDK_LIBRARIES}
+        )
+add_dependencies(waypoint_mission dji_sdk_generate_messages_cpp)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/data_capture.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/data_capture_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/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 ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+#install(DIRECTORY launch
+#        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+#        )
+
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_data_capture.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

data_capture/include/data_capture/demo_mission.h

+/** @file demo_mission.h
+ *  @version 3.3
+ *  @date May, 2017
+ *
+ *  @brief
+ *  demo sample of how to use mission APIs
+ *
+ *  @copyright 2017 DJI. All rights reserved.
+ *
+ */
+
+#ifndef DEMO_MISSION_H
+#define DEMO_MISSION_H
+
+// System includes
+#include "unistd.h"
+#include <iostream>
+#include <nlohmann/json.hpp>
+#include <fstream>
+
+// DJI SDK includes
+#include <dji_sdk/Activation.h>
+#include <dji_sdk/DroneTaskControl.h>
+#include <dji_sdk/SDKControlAuthority.h>
+#include <dji_sdk/MissionWpAction.h>
+#include <dji_sdk/MissionHpAction.h>
+#include <dji_sdk/MissionWpUpload.h>
+#include <dji_sdk/MissionHpUpload.h>
+#include <dji_sdk/MissionHpUpdateRadius.h>
+#include <dji_sdk/MissionHpUpdateYawRate.h>
+
+// SDK core library
+#include <djiosdk/dji_vehicle.hpp>
+
+// ROS includes
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+
+#define C_EARTH (double)6378137.0
+#define C_PI (double)3.141592653589793
+#define DEG2RAD(DEG) ((DEG) * ((C_PI) / (180.0)))
+
+typedef struct ServiceAck
+{
+  bool         result;
+  int          cmd_set;
+  int          cmd_id;
+  unsigned int ack_data;
+  ServiceAck(bool res, int set, int id, unsigned int ack)
+    : result(res)
+    , cmd_set(set)
+    , cmd_id(id)
+    , ack_data(ack)
+  {
+  }
+  ServiceAck()
+  {
+  }
+} ServiceAck;
+
+std::vector<DJI::OSDK::WayPointSettings>
+    loadWaypointFile(std::string filename, DJI::OSDK::float64_t altitude);
+
+bool runWaypointMission(uint8_t numWaypoints, int responseTimeout);
+
+void setWaypointDefaults(DJI::OSDK::WayPointSettings* wp);
+
+void setWaypointInitDefaults(dji_sdk::MissionWaypointTask& waypointTask);
+
+std::vector<DJI::OSDK::WayPointSettings> createWaypoints(
+  int numWaypoints, DJI::OSDK::float64_t distanceIncrement,
+  DJI::OSDK::float32_t start_alt);
+
+std::vector<DJI::OSDK::WayPointSettings> generateWaypointsPolygon(
+  DJI::OSDK::WayPointSettings* start_data, DJI::OSDK::float64_t increment,
+  int num_wp);
+
+void uploadWaypoints(std::vector<DJI::OSDK::WayPointSettings>& wp_list,
+                     int                                       responseTimeout,
+                     dji_sdk::MissionWaypointTask&             waypointTask);
+
+bool runHotpointMission(int initialRadius, int responseTimeout);
+
+void setHotpointInitDefault(dji_sdk::MissionHotpointTask& hotpointTask);
+
+ServiceAck initWaypointMission(dji_sdk::MissionWaypointTask& waypointTask);
+
+ServiceAck initHotpointMission(dji_sdk::MissionHotpointTask& hotpointTask);
+
+ServiceAck missionAction(DJI::OSDK::DJI_MISSION_TYPE type,
+                         DJI::OSDK::MISSION_ACTION   action);
+
+ServiceAck activate();
+
+ServiceAck obtainCtrlAuthority();
+
+ServiceAck takeoff();
+
+ServiceAck land();
+
+ServiceAck hotpointUpdateRadius(float radius);
+
+ServiceAck hotpointUpdateYawRate(float yawRate, int direction);
+
+void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
+
+#endif // DEMO_MISSION_H

data_capture/launch/all.launch

+<launch>
+    <include file="$(find dji_sdk)/launch/sdk.launch" />
+    <node pkg="data_capture" type="db_upload.py" name="db_uploader" respawn="true"/>
+    <node pkg="data_capture" type="Acquisition_Node.py" name="rgb_cam" respawn="true"/>
+    <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch" />
+</launch>

data_capture/launch/all_rec.launch

+<launch>
+    <node pkg="rosbag" type="record" name="record" output="screen" args="-o /home/nvidia/data_dump/cap -a"/>
+    <include file="$(find dji_sdk)/launch/sdk.launch" />
+    <node pkg="data_capture" type="db_upload.py" name="db_uploader" respawn="true"/>
+    <node pkg="data_capture" type="Acquisition_Node.py" name="rgb_cam" respawn="true"/>
+    <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch" />
+</launch>

data_capture/launch/chatter_test.launch

+<launch>
+<node name="db_streamer" pkg="data_capture" type="db_stream.py" respawn="true" output="screen"/>
+<node name="db_uploader" pkg="data_capture" type="db_upload.py" respawn="true" output="screen"/>
+</launch>

data_capture/launch/rgb_handler_test.launch

+<launch>
+  <node name="rgb_cam" pkg="data_capture" type="Acquisition_Node.py"/>
+</launch>

data_capture/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>data_capture</name>
+  <version>0.0.0</version>
+  <description>The data_capture package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="nvidia@todo.todo">nvidia</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/data_capture</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>dji_sdk</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>dji_sdk</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

data_capture/scripts/db_stream.py

+#!/usr/bin/python
+import rospy
+from sensor_msgs.msg import NavSatFix
+
+def talker():
+    pub = rospy.Publisher('chatter', NavSatFix, queue_size=10)
+    rospy.init_node('db_streamer', anonymous=True)
+    rate = rospy.Rate(3) # 3hz
+    navData = NavSatFix()
+    navData.header.frame_id = "NaN"
+    while not rospy.is_shutdown():
+        navData.latitude = 3.141
+        navData.longitude = 3.14159
+        navData.altitude = 10.3141
+        pub.publish(navData)
+        rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        talker()
+    except rospy.ROSInterruptException:
+        pass

data_capture/scripts/db_upload.py

+#!/usr/bin/python
+import rospy
+import mysql.connector
+import datetime
+import json
+from std_msgs.msg import String
+from sensor_msgs.msg import NavSatFix
+
+def callback(data, args):
+  mydb = args[0]
+  mycursor = args[1]
+  id_num = args[2]
+  table = args[3]
+
+  rospy.loginfo(rospy.get_caller_id() + "%s", data)
+
+  if data.header.seq % 10 == 0: # only update every 10th message to avoid over crowding the sql
+    gps_data = {
+      "gps" : [
+        {
+          "lat" : data.latitude,
+          "log" : data.longitude,
+          "stamp" : data.header.seq
+        },
+      ]
+    }
+    sql_data = {'gpsdata': json.dumps(gps_data),
+          'taskid':'search_0',
+          'created_at':datetime.datetime.now(),
+          'updated_at':datetime.datetime.now(),
+          'deviceid' : 'drone_0'}
+
+    sql_update = 'UPDATE ' + table + \
+    ' SET {}'.format(', '.join('{}=%s'.format(k) for k in sql_data)) +\
+    ' WHERE id = %s'
+
+    mycursor.execute(sql_update, sql_data.values()+[id_num])
+    mydb.commit()
+    
+def listener():
+
+    # In ROS, nodes are uniquely named. If two nodes with the same
+    # name are launched, the previous one is kicked off. The
+    # anonymous=True flag means that rospy will choose a unique
+    # name for our 'listener' node so that multiple listeners can
+    # run simultaneously.
+
+    table = 'app3_gpsdata'
+
+    mydb = mysql.connector.connect(
+      host="172.29.56.174",
+      user="VACSE",
+      passwd="VACSE529",
+      database="saruser"
+    )
+
+    mycursor = mydb.cursor()
+
+    id_num = 4 # id which we want to update
+
+    rospy.init_node('db_uploader', anonymous=True)
+    rospy.Subscriber("dji_sdk/gps_position", NavSatFix, callback, (mydb, mycursor, id_num, table))
+
+    # spin() simply keeps python from exiting until this node is stopped
+    rospy.spin()
+
+if __name__ == '__main__':
+    listener()

data_capture/scripts/mysql_test.py

+import mysql.connector
+import datetime
+import json
+
+#DATABASES = {
+#	'default': {
+#	'ENGINE':'django.db.backends.mysql',
+#	'NAME':'saruser',
+#	'USER': 'VACSE',
+#	'PASSWORD':'VACSE529',
+#	'HOST':'172.29.56.174',
+#	'PORT':'3306',
+#	}
+#}
+
+##stewphen sucks
+
+table = 'app3_gpsdata'
+
+mydb = mysql.connector.connect(
+	host="172.29.56.174",
+	user="VACSE",
+	passwd="VACSE529",
+	database="saruser"
+)
+
+mycursor = mydb.cursor()
+
+gps_test = {
+	"gps" : [
+		{
+			"lat" : -111.111,
+			"log" : 0.0101,
+			"stamp" : 1
+		},
+		{
+			"lat" : -0.01010,
+			"log" : -0.01010,
+			"stamp" : 2
+		},
+		{
+			"lat" : -0.01010,
+			"log" : -0.01010,
+			"stamp" : 3
+		}
+	]
+}
+sql_data = {'gpsdata': json.dumps(gps_test),
+			'taskid':'search0',
+			'created_at':datetime.datetime.now(),
+			'updated_at':datetime.datetime.now(),
+			'deviceid' : 'drone0'}
+
+id_num = 4 # id which we want to update
+
+sql_update = 'UPDATE ' + table + \
+' SET {}'.format(', '.join('{}=%s'.format(k) for k in sql_data)) +\
+' WHERE id = %s'
+
+print(sql_update)
+
+mycursor.execute(sql_update, sql_data.values()+[id_num])
+
+mydb.commit()
+
+# print(mycursor.rowcount, "Record inserted.")

data_capture/scripts/waypoint_srv_caller.py

+#!/usr/bin/env python
+import sys
+import rospy
+import math
+import time
+import numpy as np
+from dji_sdk.srv import *
+from dji_sdk.msg import * # equivalent to MissionWpUploadRequest (i think)
+
+def activate_sdk():
+    rospy.wait_for_service('dji_sdk/activation')
+    try:
+        activation_proxy = rospy.ServiceProxy('dji_sdk/activation', Activation)
+        activation_request = ActivationRequest() # should be empty anyway
+        activation_response = activation_proxy(activation_request)
+        print "Result of activation request: %s"%str(activation_response.result)
+        return activation_response
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+
+def obtain_control_authority():
+    rospy.wait_for_service('dji_sdk/sdk_control_authority')
+    try:
+        control_authority_proxy = rospy.ServiceProxy('dji_sdk/sdk_control_authority', SDKControlAuthority)
+        authority_request = SDKControlAuthorityRequest() # there is only a single field here 
+        authority_request.control_enable = 1 # take control
+        authority_response = control_authority_proxy(authority_request)
+        print "Result of control authority request: %s"%str(authority_response.result)
+        return authority_response
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+
+def drone_task(task_id = 6):
+    rospy.wait_for_service('dji_sdk/drone_task_control')
+    try:
+        task_proxy = rospy.ServiceProxy('dji_sdk/drone_task_control', DroneTaskControl)
+        task_request = DroneTaskControlRequest() # there is only a single field here 
+        task_request.task = task_id # takeoff action (4 for takeoff, 6 for landing)
+        task_response = task_proxy(task_request)
+        print "Result of task request: %s"%str(task_response.result)
+        return task_response
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+
+def get_mission_info():
+    rospy.wait_for_service('dji_sdk/mission_waypoint_getInfo')
+    try:
+        info_proxy = rospy.ServiceProxy('dji_sdk/mission_waypoint_getInfo', MissionWpGetInfo)
+        info_request = MissionWpGetInfoRequest() # empty anyway
+        info_response = info_proxy(info_request)
+        print "Information retrieved"
+        return info_response
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+
+def begin_mission(mission_action = 0):
+    rospy.wait_for_service('dji_sdk/mission_waypoint_action')
+    try:
+        action_proxy = rospy.ServiceProxy('dji_sdk/mission_waypoint_action', MissionWpAction)
+        action_request = MissionWpActionRequest()
+        action_request.action = mission_action # 0 for start, 1 for stop
+        action_response = action_proxy(action_request)
+        print "Result of task request: %s"%action_response.result
+        return action_response
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+
+
+def waypoint_upload_client(latitude_list, longitude_list):
+    rospy.wait_for_service('dji_sdk/mission_waypoint_upload')
+    print 'Latitude list: '
+    for a in latitude_list: print(a)
+    print 'longitude list: '
+    for a in longitude_list: print(a)
+    try:
+        mission_waypoint_upload_proxy = rospy.ServiceProxy('dji_sdk/mission_waypoint_upload', MissionWpUpload)
+        # build the actual request
+        req1 = MissionWpUploadRequest()
+        req1.waypoint_task.velocity_range = np.float32(10.0)
+        req1.waypoint_task.idle_velocity = np.float32(5.0)
+        req1.waypoint_task.action_on_finish = np.uint8(0)
+        req1.waypoint_task.mission_exec_times = np.uint8(1)
+        req1.waypoint_task.yaw_mode = np.uint8(0)
+        req1.waypoint_task.trace_mode = np.uint8(0)
+        req1.waypoint_task.action_on_rc_lost = np.uint8(1)
+        req1.waypoint_task.gimbal_pitch_mode = np.uint8(0)
+        # build individual waypoints
+        #
+        req1.waypoint_task.mission_waypoint = [None]*len(latitude_list)
+        for i in range(len(latitude_list)):
+            req1.waypoint_task.mission_waypoint[i] = MissionWaypoint()
+            req1.waypoint_task.mission_waypoint[i].latitude = latitude_list[i]
+            req1.waypoint_task.mission_waypoint[i].longitude = longitude_list[i]
+            req1.waypoint_task.mission_waypoint[i].altitude = 10.0
+            req1.waypoint_task.mission_waypoint[i].damping_distance = 0.0
+            req1.waypoint_task.mission_waypoint[i].target_yaw = 0
+            req1.waypoint_task.mission_waypoint[i].target_gimbal_pitch = 0
+            req1.waypoint_task.mission_waypoint[i].turn_mode = 0
+            req1.waypoint_task.mission_waypoint[i].has_action = 0
+            #~ req1.waypoint_task.mission_waypoint[i] = MissionWaypoint()
+            #~ req1.waypoint_task.mission_waypoint[i].latitude = np.float64(latitude_list[i])
+            #~ req1.waypoint_task.mission_waypoint[i].longitude = np.float64(longitude_list[i])
+            #~ req1.waypoint_task.mission_waypoint[i].altitude = np.float32(10.0)
+            #~ req1.waypoint_task.mission_waypoint[i].damping_distance = np.float32(0.0)
+            #~ req1.waypoint_task.mission_waypoint[i].target_yaw = np.int16(0)
+            #~ req1.waypoint_task.mission_waypoint[i].target_gimbal_pitch = np.int16(0)
+            #~ req1.waypoint_task.mission_waypoint[i].turn_mode = np.uint8(0)
+            #~ req1.waypoint_task.mission_waypoint[i].has_action = np.uint8(0)
+            #~ req1.waypoint_task.mission_waypoint[-1].action_time_limit = np.uint8(0)
+            #~ req1.waypoint_task.mission_waypoint[-1].waypoint_action = MissionWaypointAction()
+            #~ req1.waypoint_task.mission_waypoint[-1].waypoint_action.action_repeat = 0
+            #~ req1.waypoint_task.mission_waypoint[-1].waypoint_action.command_list = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+            #~ req1.waypoint_task.mission_waypoint[-1].waypoint_action.command_parameter = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+            # print "waypoint num %s :"%str(i)
+            # print tmp_waypoint
+            # print "---------------"
+        print "Filled out class:"
+        print req1
+        resp1 = mission_waypoint_upload_proxy(req1)
+        print "Result of waypoint task upload: %s"%str(resp1.result)
+        return resp1
+    except rospy.ServiceException, e:
+        print "Service call failed: %s"%e
+        
+def useage():
+    return "%s MissionWaypointTask"%sys.argv[0]    
+        
+if __name__ == "__main__":
+    activation_response = activate_sdk()
+    control_response = obtain_control_authority()
+    #~ takeoff_response = drone_task(4)
+    #~ if takeoff_response.result:
+        #~ print "Drone has taken off!"
+        #~ #time.sleep(5)
+    #~ else:
+        #~ print "Something went wrong, landing and exiting"
+        #~ drone_task(6)
+        #~ quit()
+        
+    start_lat = 22.542812
+    start_lon = 113.958902
+    delta_var = 0.0005
+    wp_lats = [start_lat, start_lat + delta_var, start_lat + delta_var, start_lat]
+    wp_lons = [start_lon, start_lon, start_lon + delta_var, start_lon + delta_var]
+    
+    wp_lats.extend(wp_lats)
+    wp_lons.extend(wp_lons)
+    
+    waypoint_upload_response = waypoint_upload_client(wp_lats, wp_lons)
+    time.sleep(1)
+    mission_info_response = get_mission_info()
+    print mission_info_response
+    
+    #~ begin_response = begin_mission(0)
+    #~ if begin_response.result:
+        #~ print "Mission starting!"			
+    #~ else:
+        #~ print "Something went wrong, exiting"
+        #~ drone_task(6)
+        #~ quit()
+    # land the drone where she sits
+    #landing_response = drone_task(6)
+    #if landing_response.result:
+    #    print "Drone has landed!"
+    #else:
+    #    print "Drone something went wrong, exiting"
+    #    quit()
+        
+        

data_capture/src/waypoint_info.json

+{
+    "Latitudes" : [22.542813, 22.542843, 22.542863, 22.542813, 22.542813],
+    "Longitudes" : [113.958902, 113.958902, 113.958932, 113.958942, 113.958962]
+}

velodyne-gps_imu/.gitignore

+*.pyc
+*.tar.gz
+*~
+*.user
+doc
+TAGS

velodyne-gps_imu/.travis.yml

+sudo: required
+dist: trusty
+language: generic
+compiler:
+  - gcc
+env:
+  matrix:
+    - ROS_DISTRO="indigo" ROS_REPO=ros CATKIN_CONFIG="-DCMAKE_BUILD_TYPE=Release" DOCKER_BASE_IMAGE=ros:indigo-perception
+    - ROS_DISTRO="kinetic" ROS_REPO=ros CATKIN_CONFIG="-DCMAKE_BUILD_TYPE=Release" DOCKER_BASE_IMAGE=ros:kinetic-perception
+    - ROS_DISTRO="lunar" ROS_REPO=ros CATKIN_CONFIG="-DCMAKE_BUILD_TYPE=Release" DOCKER_BASE_IMAGE=ros:lunar-perception
+    - ROS_DISTRO="melodic" ROS_REPO=ros CATKIN_CONFIG="-DCMAKE_BUILD_TYPE=Release" DOCKER_BASE_IMAGE=ros:melodic-perception
+
+install:
+  git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
+script:
+  source .ci_config/travis.sh

velodyne-gps_imu/COPYING

+
+ Copyright (C) 2010-2013 Austin Robot Technology, and others
+ All rights reserved.
+
+
+All code in this repository is released under the terms of the
+following Modified BSD License.
+
+
+Modified BSD License:
+--------------------
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+ 
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+
+    * Redistributions in binary form must reproduce the above
+      copyright notice, this list of conditions and the following
+      disclaimer in the documentation and/or other materials provided
+      with the distribution.
+
+    * Neither the names of the University of Texas at Austin, nor
+      Austin Robot Technology, nor the names of other contributors may
+      be used to endorse or promote products derived from this
+      software without specific prior written permission.
+ 
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+  POSSIBILITY OF SUCH DAMAGE.

velodyne-gps_imu/README.md

+[![Build Status](https://travis-ci.org/ros-drivers/velodyne.svg?branch=master)](https://travis-ci.org/ros-drivers/velodyne)
+
+Overview
+========
+
+Velodyne<sup>1</sup> is a collection of ROS<sup>2</sup> packages supporting `Velodyne high
+definition 3D LIDARs`<sup>3</sup>.
+
+**Warning**:
+
+  The master branch normally contains code being tested for the next
+  ROS release.  It will not always work with every previous release.
+  To check out the source for the most recent release, check out the
+  tag `velodyne-<version>` with the highest version number.
+
+The current ``master`` branch works with ROS Indigo and Kinetic.
+CI builds are currently run for Lunar and Melodic but extensive
+testing has not been completed in those environments.
+
+- <sup>1</sup>ROS: http://www.ros.org
+- <sup>2</sup>Velodyne: http://www.ros.org/wiki/velodyne
+- <sup>3</sup>`Velodyne high definition 3D LIDARs`: http://www.velodynelidar.com/lidar/lidar.aspx

velodyne-gps_imu/velodyne/CHANGELOG.rst

+Change history
+==============
+
+1.3.0 (2017-11-10)
+------------------
+* Merge pull request `#110 <https://github.com/ros-drivers/velodyne/issues/110>`_ from kmhallen/master
+  Added velodyne_laserscan package
+* Added velodyne_laserscan package and inserted into existing launch files
+* Contributors: Jack O'Quin, Joshua Whitley, Kevin Hallenbeck
+
+1.2.0 (2014-08-06)
+------------------
+
+1.1.2 (2013-11-05)
+-------------------
+
+1.1.1 (2013-07-30)
+------------------
+
+1.1.0 (2013-07-16)
+------------------
+
+1.0.1 (2013-06-15)
+------------------
+
+1.0.0 (2013-06-14)
+------------------
+
+ * Convert to catkin (`#1`_).
+ * Release to Hydro.
+
+0.9.2 (2013-07-08)
+------------------
+
+0.9.1 (2012-06-05)
+------------------
+
+0.9.0 (2012-04-03)
+------------------
+
+ * Completely revised API, anticipating a 1.0.0 release.
+ * New velodyne_driver and velodyne_pointcloud packages.
+ * Old velodyne_common and velodyne_pcl packages no longer included.
+ * Released to Electric, Fuerte and Groovy.
+
+0.2.6 (2011-02-23)
+------------------
+
+0.2.5 (2010-11-19)
+------------------
+
+ * Initial implementation of new 0.3 interfaces.
+
+0.2.0 (2010-08-17)
+------------------
+
+ * Initial release to ROS C-turtle.
+
+.. _`#1`: https://github.com/ros-drivers/velodyne/issues/1
+.. _`#4`: https://github.com/ros-drivers/velodyne/issues/4
+.. _`#7`: https://github.com/ros-drivers/velodyne/issues/7
+.. _`#8`: https://github.com/ros-drivers/velodyne/pull/8
+.. _`#9`: https://github.com/ros-drivers/velodyne/issues/9
+.. _`#10`: https://github.com/ros-drivers/velodyne/issues/10
+.. _`#11`: https://github.com/ros-drivers/velodyne/issues/11
+.. _`#12`: https://github.com/ros-drivers/velodyne/pull/12
+.. _`#13`: https://github.com/ros-drivers/velodyne/issues/13
+.. _`#14`: https://github.com/ros-drivers/velodyne/pull/14
+.. _`#17`: https://github.com/ros-drivers/velodyne/issues/17
+.. _`#18`: https://github.com/ros-drivers/velodyne/issues/18
+.. _`#20`: https://github.com/ros-drivers/velodyne/issues/20

velodyne-gps_imu/velodyne/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(velodyne)
+find_package(catkin REQUIRED)
+catkin_metapackage()

velodyne-gps_imu/velodyne/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>velodyne</name>
+  <version>1.3.0</version>
+  <description>
+    Basic ROS support for the Velodyne 3D LIDARs.
+  </description>
+  <maintainer email="jwhitley@autonomoustuff.com">Josh Whitley</maintainer>
+  <author>Jack O'Quin</author>
+  <license>BSD</license>
+  <url type="website">http://www.ros.org/wiki/velodyne</url>
+  <url type="repository">https://github.com/ros-drivers/velodyne</url>
+  <url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>velodyne_driver</exec_depend>
+  <exec_depend>velodyne_laserscan</exec_depend>
+  <exec_depend>velodyne_msgs</exec_depend>
+  <exec_depend>velodyne_pointcloud</exec_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>

velodyne-gps_imu/velodyne_driver/CHANGELOG.rst

+Change history
+==============
+
+1.3.0 (2017-11-10)
+------------------
+* Merge pull request `#129 <https://github.com/ros-drivers/velodyne/issues/129>`_ from kmhallen/pluginlib_macro
+  Modern pluginlib macro
+* Update to use non deprecated pluginlib macro
+* add launch args to support multiple devices (`#108 <https://github.com/ros-drivers/velodyne/issues/108>`_)
+* Merge pull request `#101 <https://github.com/ros-drivers/velodyne/issues/101>`_ from teosnare/master
+  velodyne_driver/src/lib/input.cc : fix for device_ip filter
+* Merge pull request `#104 <https://github.com/ros-drivers/velodyne/issues/104>`_ from altrouge/launch_options
+  Add more options in launch files.
+* Rearranged alphabetically.
+* Add more options in launch files.
+  - rpm, device_ip, port, read_once, read_fast, repeat_delay
+* velodyne_driver/src/lib/input.cc : fix for device_ip filter
+  Fix for device_ip filter in InputSocket: initialization of devip\_ for correct ip filtering in InputSocket::getPacket.
+* velodyne_driver: credit @priyankadey for VLP-16 bug fix (`#96 <https://github.com/ros-drivers/velodyne/issues/96>`_)
+* Merge pull request `#96 <https://github.com/ros-drivers/velodyne/issues/96>`_ from priyankadey/master
+  updated VLP-16 packet rate from user manual.
+* updated VLP-16 packet rate from user manual.
+  Also verified with sensor. It reduced overlap in the pointcloud
+* update change history
+* Merge pull request `#94 <https://github.com/ros-drivers/velodyne/issues/94>`_ from ros-drivers/pcap_port
+  velodyne_driver: use port number for PCAP data (`#46 <https://github.com/ros-drivers/velodyne/issues/46>`_, `#66 <https://github.com/ros-drivers/velodyne/issues/66>`_)
+* fix g++ 5.3.1 compile errors (`#94 <https://github.com/ros-drivers/velodyne/issues/94>`_)
+* merge current master (`#94 <https://github.com/ros-drivers/velodyne/issues/94>`_)
+* Merge pull request `#91 <https://github.com/ros-drivers/velodyne/issues/91>`_ from chukcha2/master
+  update velodyne_driver package description to include all models
+* update velodyne_driver package description to include all models
+* Merge pull request `#89 <https://github.com/ros-drivers/velodyne/issues/89>`_ from Tones29/feat_dynrec_driver
+  Add dynamic latency configuration to velodyne_driver
+* velodyne_driver: Add dynamic_reconfigure and time_offset correction
+  The value of time_offset is added to the calculated time stamp in live mode for each packet.
+* velodyne_driver: Make input destructors virtual
+* prepare change history for coming Indigo release (`#59 <https://github.com/ros-drivers/velodyne/issues/59>`_)
+* velodyne_driver: use port number for PCAP data (`#66 <https://github.com/ros-drivers/velodyne/issues/66>`_)
+* Merge pull request `#39 <https://github.com/ros-drivers/velodyne/issues/39>`_ from zooxco/multivelodyne
+  support for multiple velodynes
+* Merge pull request `#44 <https://github.com/ros-drivers/velodyne/issues/44>`_ from SISegwayRmp/master
+  adding driver and pointcloud support for the VLP16
+* adding the VLP16 test scripts and updating the CMakeLists to include the test file from http://download.ros.org/data/velodyne/vlp16.pcap
+* adding support for the VLP16
+* parameters to set the udp port
+* fixed missing header
+* cleanup debug line
+* parameter and code added for working with multiple velodynes
+* Contributors: Andreas Wachaja, Brice Rebsamen, Daniel Jartoux, Denis Dillenberger, Gabor Meszaros, Ilya, Jack O'Quin, Joshua Whitley, Kevin Hallenbeck, Matteo Murtas, Micho Radovnikovich, Priyanka Dey, William Woodall, jack.oquin, junior, phussey
+
+1.2.0 (2014-08-06)
+------------------
+* Fixed bug in diagnostic rate for driver (`#16
+  <https://github.com/ros-drivers/velodyne/issues/16>`_)
+* Contributors: Brice Rebsamen, Jack O'Quin
+
+1.1.2 (2013-11-05)
+-------------------
+
+ * Move unit test data to download.ros.org (`#18`_).
+ * Install missing vdump script (`#17`_).
+
+1.1.1 (2013-07-30)
+------------------
+
+ * Add support for HDL-64E S2 and S2.1 models, which were not working before (`#11`_), thanks to Gabor Meszaros (`#12`_).
+ * Add additional parameters to launch files (`#14`_).
+
+1.1.0 (2013-07-16)
+------------------
+
+ * Fix build problems due to PCL 1.7 API incompatibilities (`#8`_),
+   thanks to William Woodall.  This version also works with Groovy, as
+   long as the correct ``pcl_conversions`` is installed.
+ * Fix errors with Mac OSX compiler (`#8`_).
+ * Install ``pluginlib`` XML files (`#9`_).
+ * Install some launch and parameter files.
+ * Enable unit tests when ``CATKIN_ENABLE_TESTING`` is set (`#10`_).
+
+1.0.1 (2013-06-15)
+------------------
+
+ * Declare explicit ``pluginlib`` dependency (`#4`_).
+
+1.0.0 (2013-06-14)
+------------------
+
+ * Convert to catkin (`#1`_).
+ * Release to Hydro.
+
+0.9.2 (2013-07-08)
+------------------
+
+ * Fix Groovy build problem (`#7`_).
+
+0.9.1 (2012-06-05)
+------------------
+
+ * Driver socket read path improvements.
+ * Add unit tests with 32E data.
+ * Released to Electric, Fuerte and Groovy.
+
+0.9.0 (2012-04-03)
+------------------
+
+ * Completely revised API, anticipating a 1.0.0 release.
+ * HDL-32E device support.
+ * New velodyne_driver and velodyne_pointcloud packages.
+ * Old velodyne_common and velodyne_pcl packages no longer included.
+ * Released to Electric, Fuerte and Groovy.
+
+0.2.6 (2011-02-23)
+------------------
+
+ * Label all timing-dependent tests "realtime" so they do not run by
+   default on the build farm machines.
+
+0.2.5 (2010-11-19)
+------------------
+
+ * Initial implementation of new 0.3 interfaces.
+ * Support for ROS 1.3 `std_msgs::Header` changes.
+
+0.2.0 (2010-08-17)
+------------------
+
+ * Initial release to ROS C-turtle.
+
+.. _`#1`: https://github.com/ros-drivers/velodyne/issues/1
+.. _`#4`: https://github.com/ros-drivers/velodyne/issues/4
+.. _`#7`: https://github.com/ros-drivers/velodyne/issues/7
+.. _`#8`: https://github.com/ros-drivers/velodyne/pull/8
+.. _`#9`: https://github.com/ros-drivers/velodyne/issues/9
+.. _`#10`: https://github.com/ros-drivers/velodyne/issues/10
+.. _`#11`: https://github.com/ros-drivers/velodyne/issues/11
+.. _`#12`: https://github.com/ros-drivers/velodyne/pull/12
+.. _`#13`: https://github.com/ros-drivers/velodyne/issues/13
+.. _`#14`: https://github.com/ros-drivers/velodyne/pull/14
+.. _`#17`: https://github.com/ros-drivers/velodyne/issues/17
+.. _`#18`: https://github.com/ros-drivers/velodyne/issues/18
+.. _`#20`: https://github.com/ros-drivers/velodyne/issues/20

velodyne-gps_imu/velodyne_driver/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(velodyne_driver)
+
+set(${PROJECT_NAME}_CATKIN_DEPS 
+    diagnostic_updater
+    dynamic_reconfigure
+    nodelet
+    roscpp
+    tf
+    velodyne_msgs)
+
+find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS})
+
+# This driver uses Boost threads
+find_package(Boost REQUIRED COMPONENTS thread)
+
+# libpcap provides no pkg-config or find_package module:
+set(libpcap_LIBRARIES -lpcap)
+
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+
+# Generate dynamic_reconfigure server
+generate_dynamic_reconfigure_options(cfg/VelodyneNode.cfg)
+
+# objects needed by other ROS packages that depend on this one
+catkin_package(CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
+               INCLUDE_DIRS include
+               LIBRARIES velodyne_input)
+
+# compile the driver and input library
+add_subdirectory(src/lib)
+add_subdirectory(src/driver)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(FILES nodelet_velodyne.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY launch/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+install(PROGRAMS src/vdump
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+if (CATKIN_ENABLE_TESTING)
+
+  # these dependencies are only needed for unit testing
+  find_package(roslaunch REQUIRED)
+  find_package(rostest REQUIRED)
+
+  # Download packet capture (PCAP) files containing test data.
+  # Store them in devel-space, so rostest can easily find them.
+  catkin_download_test_data(
+    ${PROJECT_NAME}_tests_class.pcap
+    http://download.ros.org/data/velodyne/class.pcap
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
+    MD5 65808d25772101358a3719b451b3d015)
+  catkin_download_test_data(
+    ${PROJECT_NAME}_tests_32e.pcap
+    http://download.ros.org/data/velodyne/32e.pcap
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
+    MD5 e41d02aac34f0967c03a5597e1d554a9)
+  catkin_download_test_data(
+    ${PROJECT_NAME}_tests_vlp16.pcap
+    http://download.ros.org/data/velodyne/vlp16.pcap
+    DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
+    MD5 f45c2bb1d7ee358274e423ea3b66fd73)
+  
+  # unit tests
+  add_rostest(tests/pcap_node_hertz.test)
+  add_rostest(tests/pcap_nodelet_hertz.test)
+  add_rostest(tests/pcap_32e_node_hertz.test)
+  add_rostest(tests/pcap_32e_nodelet_hertz.test)
+  add_rostest(tests/pcap_vlp16_node_hertz.test)
+  add_rostest(tests/pcap_vlp16_nodelet_hertz.test)
+  
+  # parse check all the launch/*.launch files
+  roslaunch_add_file_check(launch)
+
+endif (CATKIN_ENABLE_TESTING)

velodyne-gps_imu/velodyne_driver/cfg/VelodyneNode.cfg

+#!/usr/bin/env python
+PACKAGE = "velodyne_driver"
+NODE_NAME = "velodyne_node"
+PARAMS_NAME = "VelodyneNode"
+
+from math import pi
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("time_offset", double_t,  0, "A manually calibrated offset (in seconds) to add to the timestamp before publication of a message.",
+        0.0, -1.0, 1.0)
+
+exit(gen.generate(PACKAGE, NODE_NAME, PARAMS_NAME))

velodyne-gps_imu/velodyne_driver/include/velodyne_driver/driver.h

+/* -*- mode: C++ -*- */
+/*
+ *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+ * 
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  ROS driver interface for the Velodyne 3D LIDARs
+ */
+
+#ifndef _VELODYNE_DRIVER_H_
+#define _VELODYNE_DRIVER_H_ 1
+
+#include <string>
+#include <ros/ros.h>
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <velodyne_driver/input.h>
+#include <velodyne_driver/VelodyneNodeConfig.h>
+
+namespace velodyne_driver
+{
+
+class VelodyneDriver
+{
+public:
+
+  VelodyneDriver(ros::NodeHandle node,
+                 ros::NodeHandle private_nh);
+  ~VelodyneDriver() {}
+
+  bool poll(void);
+
+private:
+
+  ///Callback for dynamic reconfigure
+  void callback(velodyne_driver::VelodyneNodeConfig &config,
+              uint32_t level);
+  /// Callback for diagnostics update for lost communication with vlp
+  void diagTimerCallback(const ros::TimerEvent&event);
+
+  ///Pointer to dynamic reconfigure service srv_
+  boost::shared_ptr<dynamic_reconfigure::Server<velodyne_driver::
+              VelodyneNodeConfig> > srv_;
+
+  // configuration parameters
+  struct
+  {
+    std::string frame_id;            ///< tf frame ID
+    std::string model;               ///< device model name
+    int    npackets;                 ///< number of packets to collect
+    double rpm;                      ///< device rotation rate (RPMs)
+    int cut_angle;                   ///< cutting angle in 1/100°
+    double time_offset;              ///< time in seconds added to each velodyne time stamp
+  } config_;
+
+  boost::shared_ptr<Input> input_;
+  ros::Publisher output_;
+
+  /** diagnostics updater */
+  ros::Timer diag_timer_;
+  diagnostic_updater::Updater diagnostics_;
+  double diag_min_freq_;
+  double diag_max_freq_;
+  boost::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic_;
+};
+
+} // namespace velodyne_driver
+
+#endif // _VELODYNE_DRIVER_H_

velodyne-gps_imu/velodyne_driver/include/velodyne_driver/input.h

+/* -*- mode: C++ -*-
+ *
+ *  Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson
+ *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2015, Jack O'Quin
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** @file
+ *
+ *  Velodyne 3D LIDAR data input classes
+ *
+ *    These classes provide raw Velodyne LIDAR input packets from
+ *    either a live socket interface or a previously-saved PCAP dump
+ *    file.
+ *
+ *  Classes:
+ *
+ *     velodyne::Input -- base class for accessing the data
+ *                      independently of its source
+ *
+ *     velodyne::InputSocket -- derived class reads live data from the
+ *                      device via a UDP socket
+ *
+ *     velodyne::InputPCAP -- derived class provides a similar interface
+ *                      from a PCAP dump file
+ */
+
+#ifndef __VELODYNE_INPUT_H
+#define __VELODYNE_INPUT_H
+
+#include <unistd.h>
+#include <stdio.h>
+#include <pcap.h>
+#include <netinet/in.h>
+
+#include <ros/ros.h>
+#include <velodyne_msgs/VelodynePacket.h>
+
+namespace velodyne_driver
+{
+  static uint16_t DATA_PORT_NUMBER = 2368;     // default data port
+  static uint16_t POSITION_PORT_NUMBER = 8308; // default position port
+
+  /** @brief Velodyne input base class */
+  class Input
+  {
+  public:
+    Input(ros::NodeHandle private_nh, uint16_t port);
+    virtual ~Input() {}
+
+    /** @brief Read one Velodyne packet.
+     *
+     * @param pkt points to VelodynePacket message
+     *
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          > 0 if incomplete packet (is this possible?)
+     */
+    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt,
+                          const double time_offset) = 0;
+
+  protected:
+    ros::NodeHandle private_nh_;
+    uint16_t port_;
+    std::string devip_str_;
+  };
+
+  /** @brief Live Velodyne input from socket. */
+  class InputSocket: public Input
+  {
+  public:
+    InputSocket(ros::NodeHandle private_nh,
+                uint16_t port = DATA_PORT_NUMBER);
+    virtual ~InputSocket();
+
+    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, 
+                          const double time_offset);
+    void setDeviceIP( const std::string& ip );
+  private:
+
+  private:
+    int sockfd_;
+    in_addr devip_;
+  };
+
+
+  /** @brief Velodyne input from PCAP dump file.
+   *
+   * Dump files can be grabbed by libpcap, Velodyne's DSR software,
+   * ethereal, wireshark, tcpdump, or the \ref vdump_command.
+   */
+  class InputPCAP: public Input
+  {
+  public:
+    InputPCAP(ros::NodeHandle private_nh,
+              uint16_t port = DATA_PORT_NUMBER,
+              double packet_rate = 0.0,
+              std::string filename="",
+              bool read_once=false,
+              bool read_fast=false,
+              double repeat_delay=0.0);
+    virtual ~InputPCAP();
+
+    virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, 
+                          const double time_offset);
+    void setDeviceIP( const std::string& ip );
+
+  private:
+    ros::Rate packet_rate_;
+    std::string filename_;
+    pcap_t *pcap_;
+    bpf_program pcap_packet_filter_;
+    char errbuf_[PCAP_ERRBUF_SIZE];
+    bool empty_;
+    bool read_once_;
+    bool read_fast_;
+    double repeat_delay_;
+  };
+
+} // velodyne_driver namespace
+
+#endif // __VELODYNE_INPUT_H

velodyne-gps_imu/velodyne_driver/include/velodyne_driver/ring_sequence.h

+/* -*- mode: C++ -*-
+ *
+ *  Copyright (C) 2010 Austin Robot Technology, Jack O'Quin
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  Velodyne HDL-64E 3D LIDAR laser ring sequence.
+ *
+ *  \author Jack O'Quin
+ */
+
+
+#ifndef __VELODYNE_RING_SEQUENCE_H
+#define __VELODYNE_RING_SEQUENCE_H
+
+namespace velodyne
+{
+  /// number of lasers
+  const int N_LASERS = 64;
+
+  /// ring sequence for device laser numbers
+  const int LASER_SEQUENCE[N_LASERS] =
+    {
+      6,  7, 10, 11,  0,  1,  4,  5,
+      8,  9, 14, 15, 18, 19, 22, 23,
+     12, 13, 16, 17, 20, 21, 26, 27,
+     30, 31,  2,  3, 24, 25, 28, 29,
+     38, 39, 42, 43, 32, 33, 36, 37,
+     40, 41, 46, 47, 50, 51, 54, 55,
+     44, 45, 48, 49, 52, 53, 58, 59,
+     62, 63, 34, 35, 56, 57, 60, 61
+    };
+
+  /// convert laser number to ring sequence (inverse of LASER_SEQUENCE)
+  const int LASER_RING[N_LASERS] =
+    {
+       4,  5, 26, 27,  6,  7,  0,  1,
+       8,  9,  2,  3, 16, 17, 10, 11,
+      18, 19, 12, 13, 20, 21, 14, 15,
+      28, 29, 22, 23, 30, 31, 24, 25,
+      36, 37, 58, 59, 38, 39, 32, 33,
+      40, 41, 34, 35, 48, 49, 42, 43,
+      50, 51, 44, 45, 52, 53, 46, 47,
+      60, 61, 54, 55, 62, 63, 56, 57
+    };
+
+} // velodyne namespace
+
+#endif // __VELODYNE_RING_SEQUENCE_H

velodyne-gps_imu/velodyne_driver/launch/nodelet_manager.launch

+<!-- -*- mode: XML -*- -->
+<!-- start velodyne_driver/DriverNodelet in a nodelet manager -->
+
+<launch>
+
+  <arg name="device_ip" default="" />
+  <arg name="frame_id" default="velodyne" />
+  <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
+  <arg name="model" default="64E" />
+  <arg name="pcap" default="" />
+  <arg name="port" default="2368" />
+  <arg name="read_fast" default="false" />
+  <arg name="read_once" default="false" />
+  <arg name="repeat_delay" default="0.0" />
+  <arg name="rpm" default="600.0" />
+  <arg name="cut_angle" default="-0.01" />
+
+  <!-- start nodelet manager -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" />
+
+  <!-- load driver nodelet into it -->
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)_driver"
+        args="load velodyne_driver/DriverNodelet $(arg manager)" >
+    <param name="device_ip" value="$(arg device_ip)" />
+    <param name="frame_id" value="$(arg frame_id)"/>
+    <param name="model" value="$(arg model)"/>
+    <param name="pcap" value="$(arg pcap)"/>
+    <param name="port" value="$(arg port)" />
+    <param name="read_fast" value="$(arg read_fast)"/>
+    <param name="read_once" value="$(arg read_once)"/>
+    <param name="repeat_delay" value="$(arg repeat_delay)"/>
+    <param name="rpm" value="$(arg rpm)"/>
+    <param name="cut_angle" value="$(arg cut_angle)"/>
+  </node>    
+
+</launch>

velodyne-gps_imu/velodyne_driver/mainpage.dox

+/**
+\mainpage
+
+\htmlinclude manifest.html
+
+ROS device driver for Velodyne 3D LIDARs. 
+
+\section read Velodyne device driver
+
+ROS device driver node that captures Velodyne 3D LIDAR data and
+publishes it to the \b velodyne_msgs/VelodyneScan topic.
+
+\subsection read_examples Examples
+
+Read the Velodyne input socket as fast as possible.  Publish each
+complete revolution to \b velodyne/rawscan.
+
+\verbatim
+$ rosrun velodyne_driver velodyne_node
+\endverbatim
+
+Read previously captured Velodyne packets from dump.pcap file.
+Publish messages to \b velodyne/rawscan at approximately 10 Hz rate.
+
+Dump files can be grabbed by libpcap, Velodyne's DSR software,
+ethereal, wireshark, tcpdump, or the velodyne_driver vdump command.
+
+\verbatim
+$ rosrun velodyne_driver velodyne_node _pcap:=dump.pcap
+\endverbatim
+
+\subsection read_names ROS names
+
+Node name: \b velodyne_node
+
+Publishes: \b velodyne_packets raw Velodyne data packets for one
+entire revolution of the device.
+
+Parameters:
+
+ - \b ~pcap (string): PCAP dump input file name (default: use real device)
+ - \b ~input/read_once (bool): if true, read input file only once
+   (default false).
+ - \b ~input/read_fast (bool): if true, read input file as fast as
+   possible (default false).
+ - \b ~input/repeat_delay (double): number of seconds to delay before
+   repeating input file (default: 0.0).
+
+\section vdump_command Vdump Command
+
+The vdump command dumps raw data from the Velodyne LIDAR in PCAP
+format.  It is a shell script wrapper with some obscure options for
+the powerful tcpdump command.
+
+Other methods of acquiring PCAP data include using tcpdump directly,
+wireshark, Velodyne's DSR software, and programming with libpcap.
+
+\subsection vdump_usage Usage
+
+\verbatim
+  rosrun velodyne_driver vdump <file_prefix> [ <interface> ]
+
+        <file_prefix>   file name to dump (with 3-digit number suffix)
+        <interface>     interface to read from (default: "eth1")
+\endverbatim
+
+\subsection vdump_examples Examples
+
+Dump Velodyne packets to a series of files named "pcap-000",
+"pcap-001", etc.  Each file will be about 100MB, holding a little more
+than 30 seconds of Velodyne packets.  Type ^C when finished.
+
+\verbatim
+$ rosrun velodyne_driver vdump pcap- eth0
+\endverbatim
+
+*/

velodyne-gps_imu/velodyne_driver/nodelet_velodyne.xml

+<library path="lib/libdriver_nodelet">
+  <class name="velodyne_driver/DriverNodelet"
+         type="velodyne_driver::DriverNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description> 
+      Publish raw Velodyne data packets.
+    </description>
+  </class>
+</library>

velodyne-gps_imu/velodyne_driver/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>velodyne_driver</name>
+  <version>1.3.0</version>
+  <description>
+    ROS device driver for Velodyne 3D LIDARs.
+  </description>
+  <maintainer email="jwhitley@autonomoustuff.com">Josh Whitley</maintainer>
+  <maintainer email="brice.rebsamen@gmail.com">Brice Rebsamen</maintainer>
+  <author>Jack O'Quin</author>
+  <author>Patrick Beeson</author>
+  <author>Michael Quinlan</author>
+  <author>Yaxin Liu </author>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/velodyne_driver</url>
+  <url type="repository">https://github.com/ros-drivers/velodyne</url>
+  <url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>diagnostic_updater</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>libpcap</depend>
+  <depend>nodelet</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>tf</depend>
+  <depend>velodyne_msgs</depend>
+
+  <test_depend>roslaunch</test_depend>
+  <test_depend>rostest</test_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_velodyne.xml"/>
+  </export>
+</package>

velodyne-gps_imu/velodyne_driver/src/driver/CMakeLists.txt

+# build the driver node
+add_executable(velodyne_node velodyne_node.cc driver.cc)
+add_dependencies(velodyne_node velodyne_driver_gencfg)
+target_link_libraries(velodyne_node
+  velodyne_input
+  ${catkin_LIBRARIES}
+  ${libpcap_LIBRARIES}
+)
+
+# build the nodelet version
+add_library(driver_nodelet nodelet.cc driver.cc)
+add_dependencies(driver_nodelet velodyne_driver_gencfg)
+target_link_libraries(driver_nodelet
+  velodyne_input
+  ${catkin_LIBRARIES}
+  ${libpcap_LIBRARIES}
+)
+
+# install runtime files
+install(TARGETS velodyne_node
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        COMPONENT main
+)
+install(TARGETS driver_nodelet
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)

velodyne-gps_imu/velodyne_driver/src/driver/driver.cc

+/*
+ *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
+ *  Copyright (C) 2009-2012 Austin Robot Technology, Jack O'Quin
+ * 
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  ROS driver implementation for the Velodyne 3D LIDARs
+ */
+
+#include <string>
+#include <cmath>
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <velodyne_msgs/VelodyneScan.h>
+
+#include "velodyne_driver/driver.h"
+
+namespace velodyne_driver
+{
+
+VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
+                               ros::NodeHandle private_nh)
+{
+  // use private node handle to get parameters
+  private_nh.param("frame_id", config_.frame_id, std::string("velodyne"));
+  std::string tf_prefix = tf::getPrefixParam(private_nh);
+  ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
+  config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
+
+  // get model name, validate string, determine packet rate
+  private_nh.param("model", config_.model, std::string("64E"));
+  double packet_rate;                   // packet frequency (Hz)
+  std::string model_full_name;
+  if ((config_.model == "64E_S2") || 
+      (config_.model == "64E_S2.1"))    // generates 1333312 points per second
+    {                                   // 1 packet holds 384 points
+      packet_rate = 3472.17;            // 1333312 / 384
+      model_full_name = std::string("HDL-") + config_.model;
+    }
+  else if (config_.model == "64E")
+    {
+      packet_rate = 2600.0;
+      model_full_name = std::string("HDL-") + config_.model;
+    }
+  else if (config_.model == "32E")
+    {
+      packet_rate = 1808.0;
+      model_full_name = std::string("HDL-") + config_.model;
+    }
+    else if (config_.model == "32C")
+    {
+      packet_rate = 1507.0;
+      model_full_name = std::string("VLP-") + config_.model;
+    }
+  else if (config_.model == "VLP16")
+    {
+      packet_rate = 754;             // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual)
+      model_full_name = "VLP-16";
+    }
+  else
+    {
+      ROS_ERROR_STREAM("unknown Velodyne LIDAR model: " << config_.model);
+      packet_rate = 2600.0;
+    }
+  std::string deviceName(std::string("Velodyne ") + model_full_name);
+
+  private_nh.param("rpm", config_.rpm, 600.0);
+  ROS_INFO_STREAM(deviceName << " rotating at " << config_.rpm << " RPM");
+  double frequency = (config_.rpm / 60.0);     // expected Hz rate
+
+  // default number of packets for each scan is a single revolution
+  // (fractions rounded up)
+  config_.npackets = (int) ceil(packet_rate / frequency);
+  private_nh.getParam("npackets", config_.npackets);
+  ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
+
+  std::string dump_file;
+  private_nh.param("pcap", dump_file, std::string(""));
+
+  double cut_angle;
+  private_nh.param("cut_angle", cut_angle, -0.01);
+  if (cut_angle < 0.0)
+  {
+    ROS_INFO_STREAM("Cut at specific angle feature deactivated.");
+  }
+  else if (cut_angle < (2*M_PI))
+  {
+      ROS_INFO_STREAM("Cut at specific angle feature activated. " 
+        "Cutting velodyne points always at " << cut_angle << " rad.");
+  }
+  else
+  {
+    ROS_ERROR_STREAM("cut_angle parameter is out of range. Allowed range is "
+    << "between 0.0 and 2*PI or negative values to deactivate this feature.");
+    cut_angle = -0.01;
+  }
+
+  // Convert cut_angle from radian to one-hundredth degree, 
+  // which is used in velodyne packets
+  config_.cut_angle = int((cut_angle*360/(2*M_PI))*100);
+
+  int udp_port;
+  private_nh.param("port", udp_port, (int) DATA_PORT_NUMBER);
+
+  // Initialize dynamic reconfigure
+  srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_driver::
+    VelodyneNodeConfig> > (private_nh);
+  dynamic_reconfigure::Server<velodyne_driver::VelodyneNodeConfig>::
+    CallbackType f;
+  f = boost::bind (&VelodyneDriver::callback, this, _1, _2);
+  srv_->setCallback (f); // Set callback function und call initially
+
+  // initialize diagnostics
+  diagnostics_.setHardwareID(deviceName);
+  const double diag_freq = packet_rate/config_.npackets;
+  diag_max_freq_ = diag_freq;
+  diag_min_freq_ = diag_freq;
+  ROS_INFO("expected frequency: %.3f (Hz)", diag_freq);
+
+  using namespace diagnostic_updater;
+  diag_topic_.reset(new TopicDiagnostic("velodyne_packets", diagnostics_,
+                                        FrequencyStatusParam(&diag_min_freq_,
+                                                             &diag_max_freq_,
+                                                             0.1, 10),
+                                        TimeStampStatusParam()));
+  diag_timer_ = private_nh.createTimer(ros::Duration(0.2), &VelodyneDriver::diagTimerCallback,this);
+
+  // open Velodyne input device or file
+  if (dump_file != "")                  // have PCAP file?
+    {
+      // read data from packet capture file
+      input_.reset(new velodyne_driver::InputPCAP(private_nh, udp_port,
+                                                  packet_rate, dump_file));
+    }
+  else
+    {
+      // read data from live socket
+      input_.reset(new velodyne_driver::InputSocket(private_nh, udp_port));
+    }
+
+  // raw packet output topic
+  output_ =
+    node.advertise<velodyne_msgs::VelodyneScan>("velodyne_packets", 10);
+}
+
+/** poll the device
+ *
+ *  @returns true unless end of file reached
+ */
+bool VelodyneDriver::poll(void)
+{
+  // Allocate a new shared pointer for zero-copy sharing with other nodelets.
+  velodyne_msgs::VelodyneScanPtr scan(new velodyne_msgs::VelodyneScan);
+
+  if( config_.cut_angle >= 0) //Cut at specific angle feature enabled
+  {
+    scan->packets.reserve(config_.npackets);
+    velodyne_msgs::VelodynePacket tmp_packet;
+    while(true)
+    {
+      while(true)
+      {
+        int rc = input_->getPacket(&tmp_packet, config_.time_offset);
+        if (rc == 0) break;       // got a full packet?
+        if (rc < 0) return false; // end of file reached?
+      }
+      scan->packets.push_back(tmp_packet);
+
+      static int last_azimuth = -1;
+      // Extract base rotation of first block in packet
+      std::size_t azimuth_data_pos = 100*0+2;
+      int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos]));
+
+      //if first packet in scan, there is no "valid" last_azimuth
+      if (last_azimuth == -1) {
+      	 last_azimuth = azimuth;
+      	 continue;
+      }
+      if((last_azimuth < config_.cut_angle && config_.cut_angle <= azimuth)
+      	 || ( config_.cut_angle <= azimuth && azimuth < last_azimuth)
+      	 || (azimuth < last_azimuth && last_azimuth < config_.cut_angle))
+      {
+        last_azimuth = azimuth;
+        break; // Cut angle passed, one full revolution collected
+      }
+      last_azimuth = azimuth;
+    }
+  }
+  else // standard behaviour
+  {
+  // Since the velodyne delivers data at a very high rate, keep
+  // reading and publishing scans as fast as possible.
+    scan->packets.resize(config_.npackets);
+    for (int i = 0; i < config_.npackets; ++i)
+    {
+      while (true)
+        {
+          // keep reading until full packet received
+          int rc = input_->getPacket(&scan->packets[i], config_.time_offset);
+          if (rc == 0) break;       // got a full packet?
+          if (rc < 0) return false; // end of file reached?
+        }
+    }
+  }
+
+  // publish message using time of last packet read
+  ROS_DEBUG("Publishing a full Velodyne scan.");
+  scan->header.stamp = scan->packets.back().stamp;
+  scan->header.frame_id = config_.frame_id;
+  output_.publish(scan);
+
+  // notify diagnostics that a message has been published, updating
+  // its status
+  diag_topic_->tick(scan->header.stamp);
+  diagnostics_.update();
+
+  return true;
+}
+
+void VelodyneDriver::callback(velodyne_driver::VelodyneNodeConfig &config,
+              uint32_t level)
+{
+  ROS_INFO("Reconfigure Request");
+  config_.time_offset = config.time_offset;
+}
+
+void VelodyneDriver::diagTimerCallback(const ros::TimerEvent &event)
+{
+  (void)event;
+  // Call necessary to provide an error when no velodyne packets are received
+  diagnostics_.update();
+}
+
+} // namespace velodyne_driver

velodyne-gps_imu/velodyne_driver/src/driver/nodelet.cc

+/*
+ *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+ * 
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  ROS driver nodelet for the Velodyne 3D LIDARs
+ */
+
+#include <string>
+#include <boost/thread.hpp>
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include "velodyne_driver/driver.h"
+
+namespace velodyne_driver
+{
+
+class DriverNodelet: public nodelet::Nodelet
+{
+public:
+
+  DriverNodelet():
+    running_(false)
+  {}
+
+  ~DriverNodelet()
+  {
+    if (running_)
+      {
+        NODELET_INFO("shutting down driver thread");
+        running_ = false;
+        deviceThread_->join();
+        NODELET_INFO("driver thread stopped");
+      }
+  }
+
+private:
+
+  virtual void onInit(void);
+  virtual void devicePoll(void);
+
+  volatile bool running_;               ///< device thread is running
+  boost::shared_ptr<boost::thread> deviceThread_;
+
+  boost::shared_ptr<VelodyneDriver> dvr_; ///< driver implementation class
+};
+
+void DriverNodelet::onInit()
+{
+  // start the driver
+  dvr_.reset(new VelodyneDriver(getNodeHandle(), getPrivateNodeHandle()));
+
+  // spawn device poll thread
+  running_ = true;
+  deviceThread_ = boost::shared_ptr< boost::thread >
+    (new boost::thread(boost::bind(&DriverNodelet::devicePoll, this)));
+}
+
+/** @brief Device poll thread main loop. */
+void DriverNodelet::devicePoll()
+{
+  while(ros::ok())
+    {
+      // poll device until end of file
+      running_ = dvr_->poll();
+      if (!running_)
+        break;
+    }
+  running_ = false;
+}
+
+} // namespace velodyne_driver
+
+// Register this plugin with pluginlib.  Names must match nodelet_velodyne.xml.
+//
+// parameters are: class type, base class type
+PLUGINLIB_EXPORT_CLASS(velodyne_driver::DriverNodelet, nodelet::Nodelet)

velodyne-gps_imu/velodyne_driver/src/driver/velodyne_node.cc

+/*
+ *  Copyright (C) 2012 Austin Robot Technology, Jack O'Quin
+ * 
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/** \file
+ *
+ *  ROS driver node for the Velodyne 3D LIDARs.
+ */
+
+#include <ros/ros.h>
+#include "velodyne_driver/driver.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "velodyne_node");
+  ros::NodeHandle node;
+  ros::NodeHandle private_nh("~");
+
+  // start the driver
+  velodyne_driver::VelodyneDriver dvr(node, private_nh);
+
+  // loop until shut down or end of file
+  while(ros::ok() && dvr.poll())
+    {
+      ros::spinOnce();
+    }
+
+  return 0;
+}

velodyne-gps_imu/velodyne_driver/src/lib/CMakeLists.txt

+add_library(velodyne_input input.cc)
+target_link_libraries(velodyne_input
+  ${catkin_LIBRARIES}
+  ${libpcap_LIBRARIES}
+)
+if(catkin_EXPORTED_TARGETS)
+  add_dependencies(velodyne_input ${catkin_EXPORTED_TARGETS})
+endif()
+
+install(TARGETS velodyne_input
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)

velodyne-gps_imu/velodyne_driver/src/vdump

+#!/bin/bash
+
+# dump velodyne packets
+# $Id: vdump 8892 2009-10-24 15:13:57Z joq $
+
+if [ x$1 = x ]
+then	echo -e "usage:\t`basename $0` file-prefix [ interface ]"
+	echo -e "\n\tfile-prefix is completed with a three-digit number"
+	echo -e "\tinterface default is 'eth0'\n"
+	exit 9
+fi
+
+IF=${2:-eth0}
+ID=`id -un`
+
+echo "acquiring packets on $IF for user $ID; press ^C when done"
+
+sudo /usr/sbin/tcpdump -i $IF -Z $ID -s 0 -C 100 -W 999 -w $1

velodyne-gps_imu/velodyne_driver/tests/diagnostic_agg.yaml

+## Diagnostic aggregator parameters for testing Velodyne diagnostics.
+#
+#  $ rosparam load $(rospack find velodyne_driver)/tests/diagnostic_agg.yaml
+#  $ rosrun diagnostic_aggregator aggregator_node
+#
+diagnostic_aggregator:
+  analyzers:
+    sensors:
+      type: diagnostic_aggregator/AnalyzerGroup
+      path: Sensors
+      analyzers:
+        velodyne:
+          type: diagnostic_aggregator/GenericAnalyzer
+          path: Velodyne HDL
+          timeout: 5.0
+          find_and_remove_prefix: velodyne_nodelet_manager
+          num_items: 1

velodyne-gps_imu/velodyne_driver/tests/pcap_32e_node_hertz.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of reading Velodyne 32E PCAP data -->
+
+<launch>
+
+  <!-- start read with example PCAP file -->
+  <node pkg="velodyne_driver" type="velodyne_node" name="velodyne_node">
+    <param name="model" value="32E"/>
+    <param name="pcap" value="$(find velodyne_driver)/tests/32e.pcap"/>
+  </node>
+
+  <test test-name="pcap_32e_node_hertz_test" pkg="rostest"
+        type="hztest" name="hztest_packets_node_32e" >
+    <param name="hz" value="10.0" />
+    <param name="hzerror" value="3.0" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="velodyne_packets" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+  <test test-name="node_diagnostics_test"
+        pkg="rostest" type="hztest" name="hztest_diag_node_32e">
+    <param name="hz" value="1.0" />
+    <param name="hzerror" value="0.5" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="diagnostics" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_driver/tests/pcap_32e_nodelet_hertz.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of publishing a PointCloud from 32E PCAP data. -->
+
+<launch>
+
+  <!-- start nodelet manager, driver and pointcloud nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="32E"/>
+    <arg name="pcap" value="$(find velodyne_driver)/tests/32e.pcap"/>
+  </include>
+
+  <!-- verify PointCloud publication rate -->
+  <test test-name="pcap_32e_nodelet_hertz_test" pkg="rostest"
+        type="hztest" name="hztest_packets_nodelet_32e" >
+    <param name="hz" value="10.0" />
+    <param name="hzerror" value="3.0" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="velodyne_packets" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+  <test test-name="nodelet_diagnostics_test"
+        pkg="rostest" type="hztest" name="hztest_diag_nodelet_32e" >
+    <param name="hz" value="1.0" />
+    <param name="hzerror" value="0.5" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="diagnostics" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_driver/tests/pcap_node_hertz.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of reading Velodyne 64E PCAP files -->
+
+<launch>
+
+  <!-- start read with example PCAP file -->
+  <node pkg="velodyne_driver" type="velodyne_node" name="velodyne_node">
+    <param name="pcap" value="$(find velodyne_driver)/tests/class.pcap"/>
+  </node>
+
+  <test test-name="pcap_node_hertz_test" pkg="rostest"
+        type="hztest" name="hztest_packets_node_64e" >
+    <param name="hz" value="10.0" />
+    <param name="hzerror" value="3.0" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="velodyne_packets" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+  <test test-name="node_diagnostics_test"
+        pkg="rostest" type="hztest" name="hztest_diag_node_64e">
+    <param name="hz" value="1.0" />
+    <param name="hzerror" value="0.5" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="diagnostics" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_driver/tests/pcap_nodelet_hertz.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of publishing a PointCloud from 64E PCAP data. -->
+
+<launch>
+
+  <!-- start nodelet manager, driver and pointcloud nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="pcap" value="$(find velodyne_driver)/tests/class.pcap"/>
+  </include>
+
+  <!-- verify PointCloud publication rate -->
+  <test test-name="pcap_nodelet_hertz_test" pkg="rostest"
+        type="hztest" name="hztest_packets_nodelet_64e" >
+    <param name="hz" value="10.0" />
+    <param name="hzerror" value="3.0" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="velodyne_packets" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+  <test test-name="nodelet_diagnostics_test"
+        pkg="rostest" type="hztest" name="hztest_diag_nodelet_64e" >
+    <param name="hz" value="1.0" />
+    <param name="hzerror" value="0.5" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="diagnostics" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_driver/tests/pcap_vlp16_node_hertz.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of reading Velodyne VLP16 PCAP data -->
+
+<launch>
+
+  <!-- start read with example PCAP file -->
+  <node pkg="velodyne_driver" type="velodyne_node" name="velodyne_node">
+    <param name="model" value="VLP16"/>
+    <param name="pcap" value="$(find velodyne_driver)/tests/vlp16.pcap"/>
+  </node>
+
+  <test test-name="pcap_vlp16_node_hertz_test" pkg="rostest"
+        type="hztest" name="hztest_packets_node_vlp16" >
+    <param name="hz" value="10.0" />
+    <param name="hzerror" value="3.0" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="velodyne_packets" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+  <test test-name="node_diagnostics_test"
+        pkg="rostest" type="hztest" name="hztest_diag_node_vlp16">
+    <param name="hz" value="1.0" />
+    <param name="hzerror" value="0.5" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="diagnostics" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_driver/tests/pcap_vlp16_nodelet_hertz.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of publishing a PointCloud from VLP16 PCAP data. -->
+
+<launch>
+
+  <!-- start nodelet manager, driver and pointcloud nodelets -->
+  <include file="$(find velodyne_driver)/launch/nodelet_manager.launch">
+    <arg name="model" value="VLP16"/>
+    <arg name="pcap" value="$(find velodyne_driver)/tests/vlp16.pcap"/>
+  </include>
+
+  <!-- verify PointCloud publication rate -->
+  <test test-name="pcap_vlp16_nodelet_hertz_test" pkg="rostest"
+        type="hztest" name="hztest_packets_nodelet_vlp16" >
+    <param name="hz" value="10.0" />
+    <param name="hzerror" value="3.0" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="velodyne_packets" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+  <test test-name="nodelet_diagnostics_test"
+        pkg="rostest" type="hztest" name="hztest_diag_nodelet_vlp16" >
+    <param name="hz" value="1.0" />
+    <param name="hzerror" value="0.5" />
+    <param name="test_duration" value="5.0" />    
+    <param name="topic" value="diagnostics" />  
+    <param name="wait_time" value="2.0" />  
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_gps_imu/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(velodyne_gps_imu)
+
+set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") #or -std=c++11
+
+find_package(catkin REQUIRED COMPONENTS
+  sensor_msgs
+  nmea_msgs
+  roscpp
+)
+
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES velodyne_gps_imu
+#  CATKIN_DEPENDS geometry_msgs roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp executable
+add_executable(gpsimu_driver src/gpsimu_driver.cc)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(gpsimu_driver
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+install(TARGETS gpsimu_driver
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+

velodyne-gps_imu/velodyne_gps_imu/package.xml

+<?xml version="1.0"?>
+<package>
+  <name>velodyne_gps_imu</name>
+  <version>0.0.0</version>
+  <description>The Velodyne GPS/IMU driver</description>
+  <maintainer email="dillenberger@uni-koblenz.de">Denis Dillenberger</maintainer>
+  <license>BSD</license>
+  <!-- <url type="website">http://wiki.ros.org/velodyne_gps_imu</url> -->
+  <author email="dillenberger@uni-koblenz.de">Denis Dillenberger</author> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>nmea_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>nmea_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <export>
+  </export>
+</package>

velodyne-gps_imu/velodyne_gps_imu/src/gpsimu_driver.h

+/* -*- mode: C++ -*-
+ *
+ *  Copyright (C) 2015 Denis Dillenberger, Rich Mattes
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+
+#ifndef IMUGPS_NODE_H
+#define IMUGPS_NODE_H
+
+#include <ros/ros.h>
+
+#include <string>
+#include <iostream>
+#include <mutex>
+#include <condition_variable>
+#include <array>
+#include <boost/bind.hpp>
+#include <boost/asio.hpp>
+#include <boost/thread.hpp>
+namespace velodyne_packet_structs
+{
+  #pragma pack(1)
+
+struct GyroTempAccelBlockRaw
+{
+    uint16_t gyro;
+    uint16_t temp;
+    uint16_t accel_x;
+    uint16_t accel_y;
+};
+
+struct VelodynePositioningPacketRaw
+{
+    char _not_used1[14];
+    GyroTempAccelBlockRaw gyro_temp_accel[3];
+    char _not_used2[160];
+    uint32_t gps_timestamp;
+    char _not_used3[4];
+    char nmea_sentence[72];
+    char _not_used4[234];
+};
+
+struct GyroTempAccelBlock
+{
+    float gyro;
+    float temp;
+    float accel_x;
+    float accel_y;
+};
+
+
+struct VelodynePositioningPacket
+{
+    GyroTempAccelBlock gyro_temp_accel[3];
+    GyroTempAccelBlock gyro_temp_accel_xyz[3];
+    double gps_timestamp;
+    char nmea_sentence[72];
+};
+
+#pragma pack()
+}
+
+class GpsImuDriver
+{
+public:
+  //! Open UDP port 8308 and listen on it
+  GpsImuDriver();
+
+  //! Disconnect cleanly
+  ~GpsImuDriver();
+
+  //! Bind socket to UDP Port, init IO thread
+  //! @return true on success, false otherwise
+  bool bind(const int udp_port);
+
+  //! Return connection status
+  bool isConnected() const { return is_connected_; }
+
+  //! Disconnect and cleanup
+  void disconnect();
+
+private:
+  //! Private NodeHandle
+  ros::NodeHandle nh_;
+
+  //! NMEA sentence publisher
+  ros::Publisher nmea_pub;
+
+  //! IMU data publisher
+  ros::Publisher imu_pub;
+
+  //! GPS time publisher
+  ros::Publisher gpstime_pub;
+
+  //! GPS temperature publisher
+  ros::Publisher temperature_pub;
+
+  //! IP of LIDAR, only packets from that address are accepted
+  std::string devip_;
+
+  //! IP of LIDAR, only packets from that address are accepted
+  int udpport_;
+
+  //! Asynchronous callback function, called if data has been reveived by the UDP socket
+  void handleSocketRead(const boost::system::error_code& error, std::size_t bytes_transferred);
+
+  //! Start asynchronous receiving
+  void asyncReceiveFrom();
+
+  //! Try to read and parse next packet from the buffer
+  //! @returns True if a packet has been parsed, false otherwise
+  bool handlePacket(velodyne_packet_structs::VelodynePositioningPacketRaw &vppr);
+
+  //! Event handler thread
+  boost::thread io_service_thread_;
+  boost::asio::io_service io_service_;
+
+  //! Receiving socket
+  boost::asio::ip::udp::socket* udp_socket_;
+
+  //! Endpoint in case of UDP receiver
+  boost::asio::ip::udp::endpoint udp_endpoint_;
+
+  //! Internal connection state
+  bool is_connected_;
+
+  //! Buffer of UDP receiver
+  std::array< char, 65536 > udp_buffer_;
+
+  //! time in seconds since epoch, when last data was received from sensor
+  //! can be used as sensor data watchdog
+  double last_data_time_;
+
+  //! time of the last gps timestamp in the raw packet message.
+  //! Used to detect hour rollover
+  uint32_t last_gps_timestamp_;
+
+  //! time in seconds since epoch of the top of the current hour
+  //! derived from NMEA string in raw packets, used to offset
+  //! time since hour gps_timestamp in raw packets.
+  time_t hour_time_;
+};
+
+#endif // IMUGPS_NODE_H

velodyne-gps_imu/velodyne_laserscan/CHANGELOG.rst

+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package velodyne_laserscan
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.3.0 (2017-11-10)
+------------------
+* Merge pull request `#110 <https://github.com/ros-drivers/velodyne/issues/110>`_ from kmhallen/master
+  Added velodyne_laserscan package
+* Added tests for velodyne_laserscan
+* Fixed validating PointCloud2 field types
+* Package.xml format version 2
+* Merge pull request `#1 <https://github.com/ros-drivers/velodyne/issues/1>`_ from volkandre/master
+  Fixed bug. Laserscans now cover full 360 degrees.
+* Fixed bug. Laserscans now cover full 360 degrees.
+* Added velodyne_laserscan package and inserted into existing launch files
+* Contributors: Joshua Whitley, Kevin Hallenbeck, kmhallen, volkandre

velodyne-gps_imu/velodyne_laserscan/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(velodyne_laserscan)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  nodelet
+  sensor_msgs
+  dynamic_reconfigure
+)
+
+generate_dynamic_reconfigure_options(
+  cfg/VelodyneLaserScan.cfg
+)
+
+catkin_package(CATKIN_DEPENDS
+  sensor_msgs
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/VelodyneLaserScan.cpp
+  src/nodelet.cpp
+)
+add_dependencies(${PROJECT_NAME}
+  ${PROJECT_NAME}_gencfg
+)
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_node
+  src/node.cpp
+)
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+  ${PROJECT_NAME}
+)
+
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(FILES nodelets.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+if (CATKIN_ENABLE_TESTING)
+  add_subdirectory(tests)
+endif()
+

velodyne-gps_imu/velodyne_laserscan/cfg/VelodyneLaserScan.cfg

+#! /usr/bin/env python
+ 
+PACKAGE='velodyne_laserscan'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                    Type      Lvl   Description                                     Default   Min     Max
+gen.add("ring",                 int_t,    0,    "Ring to extract as laser scan (-1 default)",   -1,       -1,     31)
+gen.add("resolution",           double_t, 0,    "Laser scan angular resolution (rad)",          0.007,    0.001,  0.05)
+
+exit(gen.generate(PACKAGE, PACKAGE, "VelodyneLaserScan"))

velodyne-gps_imu/velodyne_laserscan/include/velodyne_laserscan/VelodyneLaserScan.h

+#ifndef VELODYNELASERSCAN_H
+#define VELODYNELASERSCAN_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/lock_guard.hpp>
+
+#include <dynamic_reconfigure/server.h>
+#include <velodyne_laserscan/VelodyneLaserScanConfig.h>
+
+namespace velodyne_laserscan {
+
+class VelodyneLaserScan
+{
+public:
+  VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv);
+
+private:
+  boost::mutex connect_mutex_;
+  void connectCb();
+  void recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
+
+  ros::NodeHandle nh_;
+  ros::Subscriber sub_;
+  ros::Publisher pub_;
+
+  VelodyneLaserScanConfig cfg_;
+  dynamic_reconfigure::Server<VelodyneLaserScanConfig> srv_;
+  void reconfig(VelodyneLaserScanConfig& config, uint32_t level);
+
+  unsigned int ring_count_;
+};
+
+}
+
+#endif // VELODYNELASERSCAN_H

velodyne-gps_imu/velodyne_laserscan/nodelets.xml

+<library path="libvelodyne_laserscan">
+  <class name="velodyne_laserscan/LaserScanNodelet"
+         type="velodyne_laserscan::LaserScanNodelet"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Extract a single ring from a Velodyne PointCloud2 and publish
+      as a LaserScan.
+    </description>
+  </class>
+</library>

velodyne-gps_imu/velodyne_laserscan/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>velodyne_laserscan</name>
+  <version>1.3.0</version>
+  <description>
+    Extract a single ring of a Velodyne PointCloud2 and publish it as a LaserScan message
+  </description>
+  <maintainer email="jwhitley@autonomoustuff.com">Josh Whitley</maintainer>
+  <author>Micho Radovnikovich</author>
+  <author>Kevin Hallenbeck</author>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/velodyne_laserscan</url>
+  <url type="repository">https://github.com/ros-drivers/velodyne</url>
+  <url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>nodelet</depend>
+  <depend>sensor_msgs</depend>
+  <depend>dynamic_reconfigure</depend>
+
+  <test_depend>roslaunch</test_depend>
+  <test_depend>rostest</test_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelets.xml" />
+  </export>
+</package>

velodyne-gps_imu/velodyne_laserscan/src/VelodyneLaserScan.cpp

+#include "velodyne_laserscan/VelodyneLaserScan.h"
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace velodyne_laserscan {
+  
+VelodyneLaserScan::VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv) :
+    ring_count_(0), nh_(nh), srv_(nh_priv)
+{
+  ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this);
+  pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, connect_cb, connect_cb);
+  
+  srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));
+}
+
+void VelodyneLaserScan::connectCb()
+{
+  boost::lock_guard<boost::mutex> lock(connect_mutex_);
+  if (!pub_.getNumSubscribers()) {
+    sub_.shutdown();
+  } else if (!sub_) {
+    sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this);
+  }
+}
+
+void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
+{
+  // Latch ring count
+  if (!ring_count_) {
+    // Check for PointCloud2 field 'ring'
+    bool found = false;
+    for (size_t i = 0; i < msg->fields.size(); i++) {
+      if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16) {
+        if (msg->fields[i].name == "ring") {
+          found = true;
+          break;
+        }
+      }
+    }
+    if (!found) {
+      ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
+      return;
+    }
+    for (sensor_msgs::PointCloud2ConstIterator<uint16_t> it(*msg, "ring"); it != it.end(); ++it) {
+      const uint16_t ring = *it;
+      if (ring + 1 > ring_count_) {
+        ring_count_ = ring + 1;
+      }
+    }
+    if (ring_count_) {
+      ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_);
+    } else {
+      ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
+      return;
+    }
+  }
+
+  // Select ring to use
+  uint16_t ring;
+  if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_)) {
+    // Default to ring closest to being level for each known sensor
+    if (ring_count_ > 32) {
+      ring = 57; // HDL-64E
+    } else if (ring_count_ > 16) {
+      ring = 23; // HDL-32E
+    } else {
+      ring = 8; // VLP-16
+    }
+  } else {
+    ring = cfg_.ring;
+  }
+  ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring);
+
+  // Load structure of PointCloud2
+  int offset_x = -1;
+  int offset_y = -1;
+  int offset_z = -1;
+  int offset_i = -1;
+  int offset_r = -1;
+  for (size_t i = 0; i < msg->fields.size(); i++) {
+    if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32) {
+      if (msg->fields[i].name == "x") {
+        offset_x = msg->fields[i].offset;
+      } else if (msg->fields[i].name == "y") {
+        offset_y = msg->fields[i].offset;
+      } else if (msg->fields[i].name == "z") {
+        offset_z = msg->fields[i].offset;
+      } else if (msg->fields[i].name == "intensity") {
+        offset_i = msg->fields[i].offset;
+      }
+    } else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16) {
+      if (msg->fields[i].name == "ring") {
+        offset_r = msg->fields[i].offset;
+      }
+    }
+  }
+
+  // Construct LaserScan message
+  if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0)) {
+    const float RESOLUTION = std::abs(cfg_.resolution);
+    const size_t SIZE = 2.0 * M_PI / RESOLUTION;
+    sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
+    scan->header = msg->header;
+    scan->angle_increment = RESOLUTION;
+    scan->angle_min = -M_PI;
+    scan->angle_max = M_PI;
+    scan->range_min = 0.0;
+    scan->range_max = 200.0;
+    scan->time_increment = 0.0;
+    scan->ranges.resize(SIZE, INFINITY);
+    if ((offset_x == 0) && (offset_y == 4) && (offset_z == 8) && (offset_i == 16) && (offset_r == 20)) {
+      scan->intensities.resize(SIZE);
+      for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it) {
+        const uint16_t r = *((const uint16_t*)(&it[5])); // ring
+        if (r == ring) {
+          const float x = it[0]; // x
+          const float y = it[1]; // y
+          const float i = it[4]; // intensity
+          const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION;
+          if ((bin >= 0) && (bin < (int)SIZE)) {
+            scan->ranges[bin] = sqrtf(x * x + y * y);
+            scan->intensities[bin] = i;
+          }
+        }
+      }
+    } else {
+      ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method.");
+      if (offset_i >= 0) {
+        scan->intensities.resize(SIZE);
+        sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_i(*msg, "intensity");
+        for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i) {
+          const uint16_t r = *iter_r; // ring
+          if (r == ring) {
+            const float x = *iter_x; // x
+            const float y = *iter_y; // y
+            const float i = *iter_i; // intensity
+            const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION;
+            if ((bin >= 0) && (bin < (int)SIZE)) {
+              scan->ranges[bin] = sqrtf(x * x + y * y);
+              scan->intensities[bin] = i;
+            }
+          }
+        }
+      } else {
+        sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
+        sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
+        for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r) {
+          const uint16_t r = *iter_r; // ring
+          if (r == ring) {
+            const float x = *iter_x; // x
+            const float y = *iter_y; // y
+            const int bin = (atan2f(y, x) + (float)M_PI) / RESOLUTION;
+            if ((bin >= 0) && (bin < (int)SIZE)) {
+              scan->ranges[bin] = sqrtf(x * x + y * y);
+            }
+          }
+        }
+      }
+    }
+    pub_.publish(scan);
+  } else {
+    ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)");
+  }
+}
+
+void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level)
+{
+  cfg_ = config;
+}
+
+}

velodyne-gps_imu/velodyne_laserscan/src/node.cpp

+#include <ros/ros.h>
+#include "velodyne_laserscan/VelodyneLaserScan.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "velodyne_laserscan_node");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
+
+  // create VelodyneLaserScan class
+  velodyne_laserscan::VelodyneLaserScan n(nh, nh_priv);
+
+  // handle callbacks until shut down
+  ros::spin();
+
+  return 0;
+}

velodyne-gps_imu/velodyne_laserscan/src/nodelet.cpp

+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+#include "velodyne_laserscan/VelodyneLaserScan.h"
+
+namespace velodyne_laserscan
+{
+
+class LaserScanNodelet: public nodelet::Nodelet
+{
+public:
+  LaserScanNodelet() {}
+  ~LaserScanNodelet() {}
+
+private:
+  virtual void onInit() {
+    node_.reset(new VelodyneLaserScan(getNodeHandle(), getPrivateNodeHandle()));
+  }
+  boost::shared_ptr<VelodyneLaserScan> node_;
+};
+
+}
+
+PLUGINLIB_EXPORT_CLASS(velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);

velodyne-gps_imu/velodyne_laserscan/tests/CMakeLists.txt

+### Unit tests
+#
+#   Only configured when CATKIN_ENABLE_TESTING is true.
+
+# These dependencies are only needed for unit testing
+find_package(roslaunch REQUIRED)
+find_package(rostest REQUIRED)
+
+# C++ gtests
+#catkin_add_gtest(test_calibration test_calibration.cpp)
+#add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS})
+#target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES})
+
+# ROS rostests
+add_rostest_gtest(test_lazy_subscriber_node lazy_subscriber_node.test lazy_subscriber.cpp)
+add_dependencies(test_lazy_subscriber_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_lazy_subscriber_node ${catkin_LIBRARIES})
+
+add_rostest_gtest(test_lazy_subscriber_nodelet lazy_subscriber_nodelet.test lazy_subscriber.cpp)
+add_dependencies(test_lazy_subscriber_nodelet ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_lazy_subscriber_nodelet ${catkin_LIBRARIES})
+
+add_rostest_gtest(test_system_node system_node.test system.cpp)
+add_dependencies(test_system_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_system_node ${catkin_LIBRARIES})
+
+add_rostest_gtest(test_system_nodelet system_nodelet.test system.cpp)
+add_dependencies(test_system_nodelet ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_system_nodelet ${catkin_LIBRARIES})
+

velodyne-gps_imu/velodyne_laserscan/tests/lazy_subscriber.cpp

+/*********************************************************************
+ * C++ unit test for velodyne_laserscan
+ * Verify correct handling of subscribe and unsubscribe events
+ *********************************************************************/
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+
+// Subscriber receive callback
+void recv(const sensor_msgs::LaserScanConstPtr& msg) {}
+
+// Build and publish a minimal PointCloud2 message
+void publish(const ros::Publisher &pub) {
+  const uint32_t POINT_STEP = 32;
+  sensor_msgs::PointCloud2 msg;
+  msg.header.frame_id = "";
+  msg.header.stamp = ros::Time::now();
+  msg.fields.resize(5);
+  msg.fields[0].name = "x";
+  msg.fields[0].offset = 0;
+  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
+  msg.fields[0].count = 1;
+  msg.fields[1].name = "y";
+  msg.fields[1].offset = 4;
+  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
+  msg.fields[1].count = 1;
+  msg.fields[2].name = "z";
+  msg.fields[2].offset = 8;
+  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
+  msg.fields[2].count = 1;
+  msg.fields[3].name = "intensity";
+  msg.fields[3].offset = 16;
+  msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
+  msg.fields[3].count = 1;
+  msg.fields[4].name = "ring";
+  msg.fields[4].offset = 20;
+  msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
+  msg.fields[4].count = 1;
+  msg.data.resize(1 * POINT_STEP, 0x00);
+  msg.point_step = POINT_STEP;
+  msg.row_step = msg.data.size();
+  msg.height = 1;
+  msg.width = msg.row_step / POINT_STEP;
+  msg.is_bigendian = false;
+  msg.is_dense = true;
+  pub.publish(msg);
+}
+
+// Verify correct handling of subscribe and unsubscribe events
+TEST(Main, subscribe_unsubscribe)
+{
+  ros::NodeHandle nh;
+  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("velodyne_points", 2);
+
+  // Wait for node to startup
+  ros::WallDuration(2.0).sleep();
+  ros::spinOnce();
+  EXPECT_EQ(0, pub.getNumSubscribers());
+
+  // Subscribe to 'scan' and expect the node to subscribe to 'velodyne_points'
+  ros::Subscriber sub = nh.subscribe("scan", 2, recv);
+  for (size_t i = 10; i > 0; i--) {
+    publish(pub);
+    ros::WallDuration(0.1).sleep();
+    ros::spinOnce();
+  }
+  EXPECT_EQ(1, sub.getNumPublishers());
+  EXPECT_EQ(1, pub.getNumSubscribers());
+
+  // Unsubscribe from 'scan' and expect the node to unsubscribe from 'velodyne_points'
+  sub.shutdown();
+  for (size_t i = 10; i > 0; i--) {
+    publish(pub);
+    ros::WallDuration(0.1).sleep();
+    ros::spinOnce();
+  }
+  EXPECT_EQ(0, sub.getNumPublishers());
+  EXPECT_EQ(0, pub.getNumSubscribers());
+}
+
+// Run all the tests that were declared with TEST()
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_lazy_subscriber");
+  return RUN_ALL_TESTS();
+}

velodyne-gps_imu/velodyne_laserscan/tests/lazy_subscriber_node.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of the lazy subscriber -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan node -->
+  <node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
+
+  <!-- Start the rostest -->
+  <test test-name="test_lazy_subscriber_node" pkg="velodyne_laserscan"
+        type="test_lazy_subscriber_node" name="test_lazy_subscriber">
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_laserscan/tests/lazy_subscriber_nodelet.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of the lazy subscriber -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan nodelet -->
+  <arg name="manager" default="nodelet_manager" />
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager"  output="$(arg output)" />
+  <node pkg="nodelet" type="nodelet" name="velodyne_laserscan"  output="$(arg output)"
+        args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
+  </node>
+
+  <!-- Start the rostest -->
+  <test test-name="test_lazy_subscriber_nodelet" pkg="velodyne_laserscan"
+        type="test_lazy_subscriber_nodelet" name="test_lazy_subscriber">
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_laserscan/tests/system_node.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of the velodyne_laserscan system -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan node -->
+  <node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
+
+  <!-- Start the rostest -->
+  <test test-name="test_system_node" pkg="velodyne_laserscan"
+        type="test_system_node" name="test_system">
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_laserscan/tests/system_nodelet.test

+<!-- -*- mode: XML -*- -->
+<!-- rostest of the velodyne_laserscan system -->
+
+<launch>
+
+  <!-- Select log or screen output -->
+  <arg name="output" default="log"/> <!-- screen/log -->
+
+  <!-- Start the laserscan nodelet -->
+  <arg name="manager" default="nodelet_manager" />
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="$(arg output)" />
+  <node pkg="nodelet" type="nodelet" name="velodyne_laserscan" output="$(arg output)"
+        args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
+  </node>
+
+  <!-- Start the rostest -->
+  <test test-name="test_system_nodelet" pkg="velodyne_laserscan"
+        type="test_system_nodelet" name="test_system">
+  </test>
+
+</launch>

velodyne-gps_imu/velodyne_msgs/CHANGELOG.rst

+Change history
+==============
+
+1.3.0 (2017-11-10)
+------------------
+
+1.2.0 (2014-08-06)
+------------------
+
+1.1.2 (2013-11-05)
+-------------------
+
+1.1.1 (2013-07-30)
+------------------
+
+1.1.0 (2013-07-16)
+------------------
+
+1.0.1 (2013-06-15)
+------------------
+
+1.0.0 (2013-06-14)
+------------------
+
+ * Convert to catkin (`#1`_).
+ * Release to Hydro.
+
+0.9.2 (2013-07-08)
+------------------
+
+0.9.1 (2012-06-05)
+------------------
+
+ * Released to Electric, Fuerte and Groovy.
+
+0.9.0 (2012-04-03)
+------------------
+
+ * Completely revised API, anticipating a 1.0.0 release.
+ * Released to Electric, Fuerte and Groovy.
+
+0.2.6 (2011-02-23)
+------------------
+
+0.2.5 (2010-11-19)
+------------------
+
+ * Initial implementation of new 0.3 interfaces.
+ * Support for ROS 1.3 `std_msgs::Header` changes.
+
+0.2.0 (2010-08-17)
+------------------
+
+ * Initial release to ROS C-turtle.
+
+.. _`#1`: https://github.com/ros-drivers/velodyne/issues/1
+.. _`#4`: https://github.com/ros-drivers/velodyne/issues/4
+.. _`#7`: https://github.com/ros-drivers/velodyne/issues/7
+.. _`#8`: https://github.com/ros-drivers/velodyne/pull/8
+.. _`#9`: https://github.com/ros-drivers/velodyne/issues/9
+.. _`#10`: https://github.com/ros-drivers/velodyne/issues/10
+.. _`#11`: https://github.com/ros-drivers/velodyne/issues/11
+.. _`#12`: https://github.com/ros-drivers/velodyne/pull/12
+.. _`#13`: https://github.com/ros-drivers/velodyne/issues/13
+.. _`#14`: https://github.com/ros-drivers/velodyne/pull/14
+.. _`#17`: https://github.com/ros-drivers/velodyne/issues/17
+.. _`#18`: https://github.com/ros-drivers/velodyne/issues/18
+.. _`#20`: https://github.com/ros-drivers/velodyne/issues/20

velodyne-gps_imu/velodyne_msgs/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(velodyne_msgs)
+
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
+
+add_message_files(
+  DIRECTORY msg
+  FILES
+  VelodynePacket.msg
+  VelodyneScan.msg
+)
+generate_messages(DEPENDENCIES std_msgs)
+
+catkin_package(
+  CATKIN_DEPENDS message_runtime std_msgs
+)

velodyne-gps_imu/velodyne_msgs/mainpage.dox

+/**
+\mainpage
+\htmlinclude manifest.html
+
+The @b velodyne_msgs package collects ROS messages specific to the
+Velodyne HDL-64E 3D and HDL-64E S2 LIDARs.
+
+No other programming interfaces or ROS nodes are provided.
+
+*/

velodyne-gps_imu/velodyne_msgs/msg/VelodynePacket.msg

+# Raw Velodyne LIDAR packet.
+
+time stamp              # packet timestamp
+uint8[1206] data        # packet contents
+

velodyne-gps_imu/velodyne_msgs/msg/VelodyneScan.msg

+# Velodyne LIDAR scan packets.
+
+Header           header         # standard ROS message header
+VelodynePacket[] packets        # vector of raw packets

velodyne-gps_imu/velodyne_msgs/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>velodyne_msgs</name>
+  <version>1.3.0</version>
+  <description>
+    ROS message definitions for Velodyne 3D LIDARs.
+  </description>
+  <maintainer email="jwhitley@autonomoustuff.com">Josh Whitley</maintainer>
+  <author>Jack O'Quin</author>
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/velodyne_msgs</url>
+  <url type="repository">https://github.com/ros-drivers/velodyne</url>
+  <url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <depend>std_msgs</depend>
+
+  <exec_depend>message_runtime</exec_depend>
+</package>

velodyne-gps_imu/velodyne_pointcloud/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(velodyne_pointcloud)
+
+set(${PROJECT_NAME}_CATKIN_DEPS
+    angles
+    nodelet
+    pcl_ros
+    roscpp
+    roslib
+    sensor_msgs
+    tf
+    velodyne_driver
+    velodyne_msgs
+    dynamic_reconfigure
+)
+
+find_package(catkin REQUIRED COMPONENTS
+             ${${PROJECT_NAME}_CATKIN_DEPS} pcl_conversions)
+find_package(Boost COMPONENTS signals)
+
+# Resolve system dependency on yaml-cpp, which apparently does not
+# provide a CMake find_package() module.
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+find_path(YAML_CPP_INCLUDE_DIR
+          NAMES yaml_cpp.h
+          PATHS ${YAML_CPP_INCLUDE_DIRS})
+find_library(YAML_CPP_LIBRARY
+             NAMES YAML_CPP
+             PATHS ${YAML_CPP_LIBRARY_DIRS})
+
+link_directories(${YAML_CPP_LIBRARY_DIRS})
+
+generate_dynamic_reconfigure_options(
+  cfg/CloudNode.cfg cfg/TransformNode.cfg
+)
+
+if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
+  add_definitions(-DHAVE_NEW_YAMLCPP)
+endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
+
+include_directories(include ${catkin_INCLUDE_DIRS} 
+  ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake
+)
+
+catkin_package(
+    CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
+    INCLUDE_DIRS include
+    LIBRARIES velodyne_rawdata)
+    
+#add_executable(dynamic_reconfigure_node src/dynamic_reconfigure_node.cpp)
+#target_link_libraries(dynamic_reconfigure_node
+#   ${catkin_LIBRARIES}
+# )
+
+add_subdirectory(src/lib)
+add_subdirectory(src/conversions)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+install(FILES nodelets.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY launch/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+install(DIRECTORY params/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/params)
+install(PROGRAMS scripts/gen_calibration.py
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+  
+
+if (CATKIN_ENABLE_TESTING)
+  add_subdirectory(tests)
+endif()

velodyne-gps_imu/velodyne_pointcloud/cfg/CloudNode.cfg

+#!/usr/bin/env python
+PACKAGE = "velodyne_pointcloud"
+
+from math import pi
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("min_range", double_t, 0, "min range to publish", 0.9, 0.1, 10.0)
+gen.add("max_range", double_t, 0, "max range to publish", 130, 0.1, 200)
+gen.add("view_direction", double_t, 0, "angle defining the center of view",
+        0.0, -pi, pi)
+gen.add("view_width", double_t, 0, "angle defining the view width",
+        2*pi, 0.0, 2*pi)
+
+exit(gen.generate(PACKAGE, "cloud_node", "CloudNode"))

velodyne-gps_imu/velodyne_pointcloud/cfg/TransformNode.cfg

+#!/usr/bin/env python
+PACKAGE = "velodyne_pointcloud"
+
+from math import pi
+import dynamic_reconfigure.parameter_generator_catkin as pgc
+
+gen = pgc.ParameterGenerator()
+
+gen.add("min_range", 
+  pgc.double_t, 
+  0, 
+  "min range to publish", 
+  0.9, 0.1, 10.0)
+  
+gen.add("max_range", 
+  pgc.double_t, 
+  0, 
+  "max range to publish", 
+  130, 0.1, 200)
+  
+gen.add("view_direction", 
+  pgc.double_t, 
+  0, 
+  "angle defining the center of view",
+  0.0, -pi, pi)
+  
+gen.add("view_width", 
+  pgc.double_t, 
+  0, 
+  "angle defining the view width",
+  2*pi, 0.0, 2*pi)
+  
+gen.add("frame_id", 
+  pgc.str_t, 
+  0, 
+  "new frame of reference for point clouds", 
+  "odom")
+
+exit(gen.generate(PACKAGE, "transform_node", "TransformNode"))

velodyne-gps_imu/velodyne_pointcloud/include/velodyne_pointcloud/calibration.h

+/**
+ * \file  calibration.h 
+ *
+ * \author  Piyush Khandelwal (piyushk@cs.utexas.edu)
+ * Copyright (C) 2012, Austin Robot Technology, University of Texas at Austin
+ *
+ * License: Modified BSD License
+ *
+ * $ Id: 02/14/2012 11:25:34 AM piyushk $
+ */
+
+#ifndef __VELODYNE_CALIBRATION_H
+#define __VELODYNE_CALIBRATION_H
+
+#include <map>
+#include <string>
+
+namespace velodyne_pointcloud {
+
+  /** \brief correction values for a single laser
+   *
+   * Correction values for a single laser (as provided by db.xml from
+   * Velodyne).  Includes parameters for Velodyne HDL-64E S2.1.
+   *
+   * http://velodynelidar.com/lidar/products/manual/63-HDL64E%20S2%20Manual_Rev%20D_2011_web.pdf
+   */
+
+  /** \brief Correction information for a single laser. */
+  struct LaserCorrection {
+
+    /** parameters in db.xml */
+    float rot_correction;
+    float vert_correction;
+    float dist_correction;
+    bool two_pt_correction_available;
+    float dist_correction_x;
+    float dist_correction_y;
+    float vert_offset_correction;
+    float horiz_offset_correction;
+    int max_intensity;
+    int min_intensity;
+    float focal_distance;
+    float focal_slope;
+
+    /** cached values calculated when the calibration file is read */
+    float cos_rot_correction;              ///< cosine of rot_correction
+    float sin_rot_correction;              ///< sine of rot_correction
+    float cos_vert_correction;             ///< cosine of vert_correction
+    float sin_vert_correction;             ///< sine of vert_correction
+
+    int laser_ring;                        ///< ring number for this laser
+  };
+
+  /** \brief Calibration information for the entire device. */
+  class Calibration {
+
+  public:
+
+    std::map<int, LaserCorrection> laser_corrections;
+    int num_lasers;
+    bool initialized;
+    bool ros_info;
+
+  public:
+
+    Calibration(bool info=true):
+      initialized(false), ros_info(info) {}
+    Calibration(const std::string& calibration_file,
+                bool info=true):
+      ros_info(info)
+    {
+      read(calibration_file);
+    }
+
+    void read(const std::string& calibration_file);
+    void write(const std::string& calibration_file);
+  };
+  
+} /* velodyne_pointcloud */
+
+
+#endif /* end of include guard: __VELODYNE_CALIBRATION_H */
+
+

velodyne-gps_imu/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h

+
+
+#ifndef __DATACONTAINERBASE_H
+#define __DATACONTAINERBASE_H
+
+#include <ros/ros.h>
+
+namespace velodyne_rawdata 
+{
+  class DataContainerBase 
+  {
+  public:
+    virtual void addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& azimuth, const float& distance, const float& intensity) = 0;
+  };
+}
+#endif //__DATACONTAINERBASE_H
+