Bugfixes and Improvement

imu_an_spatial.orogen

@@ -11,14 +11,13 @@ version "0.1"
 # conventions, a common use-case would be:
 #
 using_library "imu_an_spatial"
-using_library "mb500"
+import_types_from "gps_base"
 
 # import_types_from "imu_an_spatial/CustomType.hpp"
 
 # If this project uses data types that are defined in other oroGen projects,
 # these projects should be imported there as well.
 import_types_from "base"
-import_types_from "gps_types.hh"
 
 # Declare a new task context (i.e., a component)
 #
@@ -38,23 +37,14 @@ task_context "Task" do
     property("NED2NWU", "bool", true).
         doc 'Switch IMU axis convention from NED(Advanced Navigation default) to NWU(Rock default)' 
     property("local_cartesian_origin", "base/Vector3d").doc("Origin of local cartesion coordinate system in lat/lon/height")
+    property("external_velocity_std_dev", "float", 0.001)
 
-    # An input port, i.e. an object from which the component gets data from
-    # other components' outputs
-    #
-    # Data can be retrieved using _input.read(value), which returns true if data
-    # was available, and false otherwise. _input.connected() returns if this
-    # input is connected to an output or not.
-    #input_port "input", "/std/string"
+    #Input of external velocity. e.g. odometry in robot frame
+    input_port "external_velocity_in", "/base/samples/RigidBodyState"
 
-    # An output port, i.e. an object to which the component pushes data so that
-    # it is transmitted to other components' inputs
-    #
-    # Data can be written using _output.write(value). _output.connected() returns
-    # if this output is connected to an input or not.
     output_port "imu_samples", "base/samples/IMUSensors"
     output_port "imu_pose", "base/samples/RigidBodyState"
-    output_port "gps_solution", "/gps/Solution"
+    output_port "gps_solution", "/gps_base/Solution"
 
     # If you want that component's updateHook() to be executed when the "input"
     # port gets data, uncomment this and comment the 'periodic' line

manifest.xml

@@ -16,6 +16,6 @@
   -->
   <depend package="base/types" />
   <depend package="drivers/imu_an_spatial" />
-  <depend package="drivers/mb500" />
+  <depend package="drivers/orogen/gps_base" />
   <depend package="geographic" />
 </package>

tasks/Task.cpp

@@ -15,6 +15,12 @@
 using namespace imu_an_spatial;
 using namespace GeographicLib;
 
+int an_packet_transmit(an_packet_t *an_packet)
+{
+    an_packet_encode(an_packet);
+    return SendBuf(an_packet_pointer(an_packet), an_packet_size(an_packet));
+}
+
 Task::Task(std::string const& name)
     : TaskBase(name)
 {
@@ -65,8 +71,11 @@ void Task::updateHook()
 
     base::samples::IMUSensors imu_sample;
     base::samples::RigidBodyState imu_pose;
-    gps::Solution sol;
+    gps_base::Solution sol;
+    base::samples::RigidBodyState external_velocity;
+    gps_base::Errors errors;
 
+
     if (fd_activity)
     {
         if (fd_activity->hasError())
@@ -79,6 +88,19 @@ void Task::updateHook()
         }
         else
         {
+            while(_external_velocity_in.read(external_velocity) == RTT::NewData){
+                an_packet_t* an_packet;
+                external_body_velocity_packet_t body_vel_packet;
+                memset(&body_vel_packet, 0, sizeof(external_body_velocity_packet_t));
+                body_vel_packet.velocity[0] = external_velocity.velocity.x();
+                body_vel_packet.velocity[1] = external_velocity.velocity.y();
+                body_vel_packet.velocity[2] = external_velocity.velocity.z();
+                body_vel_packet.standard_deviation = _external_velocity_std_dev.get();
+
+                an_packet = encode_external_body_velocity_packet(&body_vel_packet);
+                an_packet_transmit(an_packet);
+                an_packet_free(&an_packet);
+            }
             if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0)
             {
                 /* increment the decode buffer length by the number of bytes received */
@@ -139,14 +161,15 @@ void Task::updateHook()
 
                             gnss_fix_type_e gnss_fix_type = static_cast<gnss_fix_type_e>(system_state_packet.filter_status.b.gnss_fix_type);
                             switch(gnss_fix_type){
-                                case gnss_fix_none: sol.positionType = gps::NO_SOLUTION; break;
-                                case gnss_fix_2d: sol.positionType = gps::AUTONOMOUS_2D; break;
-                                case gnss_fix_3d: sol.positionType = gps::AUTONOMOUS; break;
-                                case gnss_fix_sbas: sol.positionType = gps::DIFFERENTIAL; break;
-                                case gnss_fix_differential: sol.positionType = gps::DIFFERENTIAL; break;
-                                case gnss_fix_rtk_float: sol.positionType = gps::RTK_FLOAT; break;
-                                case gnss_fix_rtk_fixed: sol.positionType = gps::RTK_FIXED; break;
-                                default: sol.positionType = gps::INVALID; break;
+                                case gnss_fix_none: sol.positionType = gps_base::NO_SOLUTION; break;
+                                case gnss_fix_2d: sol.positionType = gps_base::AUTONOMOUS_2D; break;
+                                case gnss_fix_3d: sol.positionType = gps_base::AUTONOMOUS; break;
+                                case gnss_fix_sbas: sol.positionType = gps_base::DIFFERENTIAL; break;
+                                case gnss_fix_differential: sol.positionType = gps_base::DIFFERENTIAL; break;
+                                case gnss_fix_omnistar: sol.positionType = gps_base::DIFFERENTIAL; break;
+                                case gnss_fix_rtk_float: sol.positionType = gps_base::RTK_FLOAT; break;
+                                case gnss_fix_rtk_fixed: sol.positionType = gps_base::RTK_FIXED; break;
+                                default: sol.positionType = gps_base::INVALID; break;
                             }
 
                             sol.latitude = system_state_packet.latitude * RADIANS_TO_DEGREES;