Add package rtkrcv

rtkrcv/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+################################################################################
+# RTKLIB
+################################################################################
+
+#INCLUDE($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake)
+
+rosbuild_find_ros_package(rtklib)
+include_directories(${rtklib_PACKAGE_PATH}/include)
+find_library(LIBRTK NAMES rtk PATHS ${rtklib_PACKAGE_PATH}/lib)
+message(${LIBRTK})
+rosbuild_add_executable(${PROJECT_NAME}
+                       src/main.cpp
+                       src/ros_options.cpp
+                       src/rtkrcv.cpp)
+target_link_libraries(${PROJECT_NAME} ${LIBRTK})

rtkrcv/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

rtkrcv/manifest.xml

@@ -0,0 +1,19 @@
+<package>
+  <description brief="rtkrcv">
+
+     GNSS RTK receiver node
+
+  </description>
+  <author>Manuel Stahl</author>
+  <license>GPLv3</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/rtkrcv</url>
+
+  <depend package="rtklib"/>
+  <depend package="roscpp"/>
+  <depend package="roslib"/>
+  <depend package="sensor_msgs"/>
+  <rosdep name="liblapack-dev"/>
+  <rosdep name="libblas-dev"/>
+
+</package>

rtkrcv/rtkrcv.launch

@@ -0,0 +1,29 @@
+<launch>
+	<node pkg="rtkrcv" type="rtkrcv" name="rtkrcv">
+		<param name="pos1_posmode"   value="single"/>    <!-- (0:single,1:dgps,2:kinematic,3:static,4:movingbase,5:fixed,6:ppp-kine,7:ppp-static) -->
+		<param name="pos1_frequency" value="l1"/>        <!-- (1:l1,2:l1+l2,3:l1+l2+l5,4:l1+l2+l5+l6,5:l1+l2+l5+l6+l7) -->
+		<param name="pos1_soltype"   value="forward"/>   <!-- (0:forward,1:backward,2:combined) -->
+		<param name="pos1_elmask"    value="5"/>         <!-- (deg) -->
+		<param name="pos1_snrmask"   value="0"/>         <!-- (dBHz) -->
+		<param name="pos1_dynamics"  value="off"/>       <!-- (0:off,1:on) -->
+		<param name="pos1_tidecorr"  value="off"/>       <!-- (0:off,1:on) -->
+		<param name="pos1_ionoopt"   value="brdc"/>      <!-- (0:off,1:brdc,2:sbas,3:dual-freq,4:est-stec,5:ionex-tec,6:qzs-brdc,7:qzs-lex,8:vtec_sf,9:vtec_ef,10:gtec) -->
+		<param name="pos1_tropopt"   value="saas"/>      <!-- (0:off,1:saas,2:sbas,3:est-ztd,4:est-ztdgrad) -->
+		<param name="pos1_sateph"    value="brdc"/>      <!-- (0:brdc,1:precise,2:brdc+sbas,3:brdc+ssrapc,4:brdc+ssrcom) -->
+		<param name="pos1_navsys"    value="1"/>         <!-- (1:gps+2:sbas+4:glo+8:gal+16:qzs+32:comp) -->
+		<param name="out_solformat"  value="enu"/>       <!-- (0:llh,1:xyz,2:enu,3:nmea) -->
+		<param name="out_timesys"    value="utc"/>       <!-- (0:gpst,1:utc,2:jst) -->
+		<param name="out_timeform"   value="hms"/>       <!-- (0:tow,1:hms) -->
+		<param name="out_timendec"   value="3"/>
+		<param name="out_degform"    value="deg"/>       <!-- (0:deg,1:dms) -->
+		<param name="out_outstat"    value="residual"/>  <!-- (0:off,1:state,2:residual) -->
+
+		<param name="inpstr1_type"   value="tcpcli"/>    <!-- (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,7:ntripcli,8:ftp,9:http) -->
+		<param name="inpstr1_path"   value=":@localhost:10000/:"/>
+		<param name="inpstr1_format" value="skytraq"/>   <!-- (0:rtcm2,1:rtcm3,2:oem4,3:oem3,4:ubx,5:ss2,6:hemis,7:skytraq,8:gw10,9:javad,15:sp3) -->
+		<param name="outstr1_type"   value="tcpsvr"/>    <!-- (0:off,1:serial,2:file,3:tcpsvr,4:tcpcli,6:ntripsvr) -->
+		<param name="outstr1_path"   value=":@:3333/:"/>
+		<param name="outstr1_format" value="enu"/>       <!-- (0:llh,1:xyz,2:enu,3:nmea) -->
+	</node>
+</launch>
+

rtkrcv/src/main.cpp

@@ -0,0 +1,40 @@
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+
+int start_rtkrcv(int argc, char **argv);
+void stop_rtkrcv();
+char get_solution(double &lat, double &lon, double &height);
+
+int
+main(int argc, char** argv)
+{
+  std::string frame_id;
+  ros::init(argc, argv, "rtklib");
+
+  ros::NodeHandle private_nh("~");
+  private_nh.param("frame_id", frame_id, std::string("antenna"));
+
+  ros::NodeHandle nh;
+  ros::Publisher pub = nh.advertise<sensor_msgs::NavSatFix>("fix", 5);
+
+  if (!start_rtkrcv(argc, argv)) return(-1);
+
+  ros::Rate r(1); // 1 Hz
+  while (ros::ok()) {
+    sensor_msgs::NavSatFix fix;
+    fix.header.stamp = ros::Time::now();
+    fix.header.frame_id = frame_id;
+    fix.status.service = fix.status.SERVICE_GPS | fix.status.SERVICE_GALILEO;
+    fix.status.status = get_solution(fix.latitude, fix.longitude, fix.altitude);
+    fix.position_covariance_type = fix.COVARIANCE_TYPE_UNKNOWN;
+    pub.publish(fix);
+
+    ros::spinOnce();
+    r.sleep();
+  }
+
+  stop_rtkrcv();
+
+  return(0);
+}
+

rtkrcv/src/ros_options.cpp

@@ -0,0 +1,19 @@
+#include <string>
+#include <algorithm>
+#include <ros/ros.h>
+#include <rtklib/rtklib.h>
+
+int loadrosopts(opt_t *opts)
+{
+  ros::NodeHandle private_nh("~");
+  for (opt_t *opt = opts; opt->var != NULL; opt++) {
+    std::string value;
+    std::string key(opt->name);
+    std::replace(key.begin(), key.end(), '-', '_');
+    private_nh.param(key, value, value);
+    if (value.empty()) continue;
+    if (!str2opt(opt, value.c_str()))
+      trace(1,"loadrosopts: invalid option value '%s' (%s), %d\n", value.c_str(), opt->name, value.length());
+  }
+  return 1;
+}