Merge pull request #60 from MapIV/fix/bag

Fix/bag

README.md

@@ -33,61 +33,62 @@ Eagleye is an open-source software for vehicle localization utilizing GNSS and I
 
 Eagleye uses vehicle speed acquired from CAN bus.
 
-### Prerequisites
+### When eagleye's GNSS input is single point positioning (spp)
+#### Prerequisites in the case of spp
 
 1. Clone and Build MapIV's fork of [RTKLIB](https://github.com/MapIV/RTKLIB/tree/rtklib_ros_bridge). You can find more details about RTKLIB [here](http://www.rtklib.com/).
 
-		sudo apt-get install gfortran  
-		cd $HOME  
+		sudo apt-get install gfortran
+		cd $HOME
 		git clone -b rtklib_ros_bridge https://github.com/MapIV/RTKLIB.git
-		cd $HOME/RTKLIB/lib/iers/gcc/  
-		make   
-		cd $HOME/RTKLIB/app  
-		make   
+		cd $HOME/RTKLIB/lib/iers/gcc/
+		make
+		cd $HOME/RTKLIB/app
+		make 
 
 2. Clone and build [rtklib_ros_bridge](https://github.com/MapIV/rtklib_ros_bridge).
 
-		cd $HOME/catkin_ws/src  
-		git clone https://github.com/MapIV/rtklib_ros_bridge.git  
-		cd ..  
-		catkin_make -DCMAKE_BUILD_TYPE=Release  
+		cd $HOME/catkin_ws/src
+		git clone https://github.com/MapIV/rtklib_ros_bridge.git
+		cd ..
+		catkin_make -DCMAKE_BUILD_TYPE=Release
 
-3. Clone and build [nmea_navsat_driver](https://github.com/MapIV/nmea_navsat_driver.git).
+3. Clone and build [nmea_comms](https://github.com/MapIV/nmea_comms.git).
 
-		cd $HOME/catkin_ws/src  
-		git clone https://github.com/MapIV/nmea_navsat_driver.git  
-		cd ..  
-		catkin_make -DCMAKE_BUILD_TYPE=Release  
+		cd $HOME/catkin_ws/src
+		git clone https://github.com/MapIV/nmea_comms.git
+		cd ..
+		catkin_make -DCMAKE_BUILD_TYPE=Release
 
 
 4. Installing dependent packages  
 
 In the case of Ubuntu18.04 melodic.   
 
-		sudo apt-get install ros-melodic-geodesy   
-		sudo apt-get install ros-melodic-can-msgs  
+		sudo apt-get install ros-melodic-geodesy 
+		sudo apt-get install ros-melodic-can-msgs
 In the case of Ubuntu16.04 kinetic.  
 
-		sudo apt-get install ros-kinetic-geodesy   
-		sudo apt-get install ros-kinetic-can-msgs  
+		sudo apt-get install ros-kinetic-geodesy
+		sudo apt-get install ros-kinetic-can-msgs
 
 5. Clone and build [eagleye](https://github.com/MapIV/eagleye.git).
 
-		cd $HOME/catkin_ws/src  
-		git clone https://github.com/MapIV/eagleye.git  
-		cd ..  
-		catkin_make -DCMAKE_BUILD_TYPE=Release  
+		cd $HOME/catkin_ws/src
+		git clone https://github.com/MapIV/eagleye.git
+		cd ..
+		catkin_make -DCMAKE_BUILD_TYPE=Release
 
 6. RTKLIB settings.
 
-Change `inpstr1-path` of `$HOME/RTKLIB/app/rtkrcv/conf/rtklib_ros_bridge_sample.conf` according to the serial device you use.
+Change `inpstr1-path` of `$HOME/RTKLIB/app/rtkrcv/conf/rtklib_ros_bridge_single.conf` according to the serial device you use.
 
 ie)
 >inpstr1-path =/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00:230400:8:n:1:off  
 
-7. nmea_navsat_driver settings.  
+7. nmea_comms settings.
 
-Change `arg name="port"` of `$HOME/catkin_ws/src/nmea_navsat_driver/launch/f9p_nmea_serial_driver.launch` according to the serial device you use.
+Change `arg name="port"` of `$HOME/catkin_ws/src/nmea_comms/launch/f9p_nmea_sentence.launch` according to the serial device you use.
 
 ie)
 >\<arg name="port" default="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_AG0JNPDS-if00-port0" />
@@ -96,30 +97,32 @@ ie)
 Configure the receiver settings using [u-center](https://www.u-blox.com/product/u-center).
 
 * UART1(Connect to RTKLIB) Enable UBX message (output rate 5Hz, baudrate 230400) ※ Set to output only RAWX and SFRBX
-* UART2(Connect to nmea_navsat_driver) Enable NMEA message (output rate 1Hz, baudrate 115200) ※ Set to output only GGA and RMC
+* UART2(Connect to nmea_comms) Enable NMEA message (output rate 1Hz, baudrate 115200) ※ Set to output only GGA and RMC
 
-[This file](https://www.dropbox.com/s/5mq9hbygnviojoh/eagleye_f9p_conf.txt?dl=0) is a sample configuration file for F9P.  
+[This file (eagleye_f9p_conf.txt)](https://www.dropbox.com/s/5mq9hbygnviojoh/eagleye_f9p_conf.txt?dl=0) is a sample configuration file for F9P.  
 Open u-center.  
 Tools/Receiver Configuration.../Load configuration "Transfer file -> GNSS"
 
+To load the configuration, change the ublox FW version to 1.10.
+
 9. IMU settings.
 
 * Output rate 50Hz
 
-10. Check the rotation direction of z axis of IMU being used. If you look from the top of the vehicle, if the left turn is positive, set "reverse_imu" to `true` in `eagleye/launch/eagleye_localization.launch`.
+10. Check the rotation direction of z axis of IMU being used. If you look from the top of the vehicle, if the left turn is positive, set "reverse_imu" to `true` in `eagleye/eagleye_rt/config/eagleye_config.yaml`.
 
-		param name="/eagleye/reverse_imu" type="bool" value="true"
+		 reverse_imu: true
 
 
-### Running eagleye node
+#### Running eagleye node in the case of spp
 
 1. Check if wheel speed (vehicle speed) is published in `/can_twist` topic.
 
 * Topic name: /can_twist
 * Message type: geometry_msgs/TwistStamped twist.liner.x
 
 
-2. Check if the IMU data is published in `/imu_raw` topic.
+2. Check if the IMU data is published in `/imu/data_raw` topic.
 
 3. Start RTKLIB.
 
@@ -134,14 +137,154 @@ Tools/Receiver Configuration.../Load configuration "Transfer file -> GNSS"
 
 		roslaunch rtklib_bridge rtklib_bridge.launch   
 
-6. Start nmea_navsat_driver.
+6. Start nmea_comms and [nmea2fix](eagleye_util/nmea2fix/README.md).
 
-		rosrun nmea_navsat_driver f9p_nmea_serial_driver.launch   
+		roslaunch nmea_comms f9p_nmea_sentence.launch
+		roslaunch nmea2fix nmea2fix.launch
 
 7. Start eagleye.
 
 		roslaunch eagleye_rt eagleye_rt.launch
 
+To visualize the eagleye output location /eagleye/fix, for example, use the following command
+
+	rosrun fix2kml fix2kml
+
+### When eagleye's GNSS input is RTK
+#### Prerequisites in the case of RTK
+
+When inputting RTK results from the F9P into the Eagleye, two F9Ps are used as follows.  
+(1) A receiver that outputs NMEA (RTK results from the F9P internal engine)  
+(2) A receiver that measures RAW data through RTKLIB and outputs Doppler velocity. 
+
+1. Clone and Build MapIV's fork of [RTKLIB](https://github.com/MapIV/RTKLIB/tree/rtklib_ros_bridge). You can find more details about RTKLIB [here](http://www.rtklib.com/).
+
+		sudo apt-get install gfortran
+		cd $HOME
+		git clone -b rtklib_ros_bridge https://github.com/MapIV/RTKLIB.git
+		cd $HOME/RTKLIB/lib/iers/gcc/
+		make
+		cd $HOME/RTKLIB/app
+		make
+
+2. Clone and build [rtklib_ros_bridge](https://github.com/MapIV/rtklib_ros_bridge).
+
+		cd $HOME/catkin_ws/src
+		git clone https://github.com/MapIV/rtklib_ros_bridge.git
+		cd ..
+		catkin_make -DCMAKE_BUILD_TYPE=Release
+
+3. Clone and build [nmea_comms](https://github.com/MapIV/nmea_comms.git).
+
+		cd $HOME/catkin_ws/src
+		git clone https://github.com/MapIV/nmea_comms.git
+		cd ..
+		catkin_make -DCMAKE_BUILD_TYPE=Release
+
+
+4. Installing dependent packages  
+
+In the case of Ubuntu18.04 melodic.   
+
+		sudo apt-get install ros-melodic-geodesy
+		sudo apt-get install ros-melodic-can-msgs
+In the case of Ubuntu16.04 kinetic.  
+
+		sudo apt-get install ros-kinetic-geodesy
+		sudo apt-get install ros-kinetic-can-msgs
+
+5. Clone and build [eagleye](https://github.com/MapIV/eagleye.git).
+
+		cd $HOME/catkin_ws/src
+		git clone https://github.com/MapIV/eagleye.git
+		cd ..
+		catkin_make -DCMAKE_BUILD_TYPE=Release
+
+6. RTKLIB settings.
+
+Change `inpstr1-path`, `inpstr2-path`, `inpstr2-format`, and `ant2-postype` of `$HOME/RTKLIB/app/rtkrcv/conf/rtklib_ros_bridge_meijo_rtk.conf` according to the serial device you use.
+
+ie)
+>inpstr1-path =/serial/by-id/usb-u-blox_AG_-_www.u-blox.com_u-blox_GNSS_receiver-if00:230400:8:n:1:off  
+>inpstr2-path =:@rtk2go.com:2101/Meijo-Ublox  
+>inpstr2-format =ubx  
+>ant2-postype       =llh        # (0:llh,1:xyz,2:single,3:posfile,4:rinexhead,5:rtcm,6:raw)  
+ant2-pos1          =35.1348599331534          # (deg|m) If ant2-postype is llh or xyz, the position of the reference station must be specified by ant2-pos1, ant2-pos2, and ant2-pos3.  
+ant2-pos2          =136.973613158051          # (deg|m)  
+ant2-pos3          =102.502548295454          # (m|m)  
+
+7. nmea_comms settings.  
+
+Change `arg name="port"` of `$HOME/catkin_ws/src/nmea_comms/launch/f9p_nmea_sentence.launch` according to the serial device you use.
+
+ie)
+>\<arg name="port" default="/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_AG0JNPDS-if00-port0" />
+
+8. GNSS receiver settings.
+Configure the receiver settings using [u-center](https://www.u-blox.com/product/u-center).
+
+The following is a sample configuration file for F9P.  
+
+(1) Settings for receivers that output aircraft that output NMEA (positioning results RTK'd by the F9P internal engine)  
+eagleye_f9p_nmea_conf.txt  
+https://www.dropbox.com/s/3viqyqutipn5dpj/eagleye_f9p_nmea_conf.txt?dl=0  
+(2) Settings for receivers that measure RAW data through RTKLIB and output Doppler velocity.  
+eagleye_f9p_raw_conf.txt  
+https://www.dropbox.com/s/acz98v30rtgbmsx/eagleye_f9p_raw_conf.txt?dl=0  
+Open u-center.  
+Tools/Receiver Configuration.../Load configuration "Transfer file -> GNSS"
+
+To load the configuration, change the ublox FW version to 1.10.
+
+9. IMU settings.
+
+* Output rate 50Hz
+
+10. Check the rotation direction of z axis of IMU being used. If you look from the top of the vehicle, if the left turn is positive, set "reverse_imu" to `true` in `eagleye/eagleye_rt/config/eagleye_config.yaml`.
+
+		 reverse_imu: true
+
+
+#### Running eagleye node in the case of RTK
+
+1. Check if wheel speed (vehicle speed) is published in `/can_twist` topic.
+
+* Topic name: /can_twist
+* Message type: geometry_msgs/TwistStamped twist.liner.x
+
+
+2. Check if the IMU data is published in `/imu/data_raw` topic.
+
+3. Start RTKLIB.
+
+		cd $HOME/RTKLIB
+		bash rtklib_ros_bridge_meijo_rtk.sh
+
+4. Check if RTKLIB is working by execute the following command in the terminal. If the RTKLIB is working correctly, positioning information is appeared continuously in the terminal.
+
+		status 0.1
+
+5. Start rtklib_ros_bridge.
+
+		roslaunch rtklib_bridge rtklib_bridge.launch
+
+6. Start nmea_comms and [nmea2fix](eagleye_util/nmea2fix/README.md).
+
+		roslaunch nmea_comms f9p_nmea_sentence.launch
+		roslaunch nmea2fix nmea2fix.launch
+
+7. Start RTKLIB str2str to send the correction information to the receiver that outputs NMEA.
+
+		$HOME/RTKLIB/app/str2str/gcc/str2str -b 1 -in <reference_station:port/mount_point> -out <port_of_rover:baudrate>
+
+8. Start eagleye.
+
+		roslaunch eagleye_rt eagleye_rt.launch
+
+To visualize the eagleye output location /eagleye/fix, for example, use the following command
+
+	rosrun fix2kml fix2kml
+
 ## Sample data
 ### ROSBAG
 

eagleye_core/navigation/include/navigation/navigation.hpp

@@ -237,11 +237,13 @@ struct TrajectoryParameter
 {
   bool reverse_imu;
   double stop_judgment_velocity_threshold;
+  double stop_judgment_yawrate_threshold;
 };
 
 struct TrajectoryStatus
 {
   int estimate_status_count;
+  double heading_last;
   double time_last;
 };
 
@@ -273,6 +275,7 @@ struct HeightStatus
   bool estimate_start_status;
   bool acceleration_SF_estimate_status;
   int data_number;
+  bool flag_reliability;
   std::vector<double> height_buffer;
   std::vector<double> height_buffer2;
   std::vector<double> relative_height_G_buffer;

eagleye_core/navigation/src/distance.cpp

@@ -30,12 +30,12 @@
 
  #include "coordinate/coordinate.hpp"
  #include "navigation/navigation.hpp"
- 
+
 void distance_estimate(const eagleye_msgs::VelocityScaleFactor velocity_scale_factor, DistanceStatus* distance_status,eagleye_msgs::Distance* distance)
 {
   if(distance_status->time_last != 0)
   {
-    distance->distance = distance->distance + velocity_scale_factor.correction_velocity.linear.x * (velocity_scale_factor.header.stamp.toSec() - distance_status->time_last);
+    distance->distance = distance->distance + velocity_scale_factor.correction_velocity.linear.x * abs((velocity_scale_factor.header.stamp.toSec() - distance_status->time_last));
     distance->status.enabled_status = distance->status.estimate_status = true;
     distance_status->time_last = velocity_scale_factor.header.stamp.toSec();
   }

eagleye_core/navigation/src/heading.cpp

@@ -80,7 +80,7 @@ void heading_estimate(rtklib_msgs::RtklibNav rtklib_nav,sensor_msgs::Imu imu,eag
     yawrate = -1 * imu.angular_velocity.z;
   }
 
-  if (heading_status->tow_last  == rtklib_nav.tow)
+  if (heading_status->tow_last  == rtklib_nav.tow || rtklib_nav.tow == 0)
   {
     gnss_status = false;
     doppler_heading_angle = 0;

eagleye_core/navigation/src/height.cpp

@@ -80,6 +80,8 @@ void pitching_estimate(const sensor_msgs::Imu imu,const sensor_msgs::NavSatFix f
     height_status->fix_time_last = fix.header.stamp.toSec();
   }
 
+  height_status->flag_reliability = false;
+
 ///  relative_height  ///
   if (velocity_scale_factor.correction_velocity.linear.x > 0 && height_status->time_last != 0)
   {
@@ -337,13 +339,15 @@ void pitching_estimate(const sensor_msgs::Imu imu,const sensor_msgs::NavSatFix f
             height_status->height_last = tmp_height;
             height->status.enabled_status = true;
             height->status.estimate_status = true;
+            height_status->flag_reliability = true;
           }
           else
           {
             height_status->height_last = tmp_height + (height_status->correction_relative_height_buffer2[height_status->data_number - 1]
               - height_status->correction_relative_height_buffer2[index[index_length - 1]]);
             height->status.enabled_status = true;
             height->status.estimate_status = true;
+            height_status->flag_reliability = false;
           }
         }
       }
@@ -363,11 +367,14 @@ void pitching_estimate(const sensor_msgs::Imu imu,const sensor_msgs::NavSatFix f
   height_status->acc_buffer.push_back((correction_acceleration_linear_x - (velocity_scale_factor.correction_velocity.linear.x-height_status->correction_velocity_x_last)/(imu.header.stamp.toSec()-height_status->time_last)));
   data_num_acc = height_status->acc_buffer.size();
 
-  if (data_num_acc > height_parameter.average_num && height_status->estimate_start_status == true)
+  if (data_num_acc > height_parameter.average_num)
   {
     height_status->acc_buffer.erase(height_status->acc_buffer.begin());
     data_num_acc--;
+  }
 
+  if (data_num_acc >= height_parameter.average_num && height_status->estimate_start_status == true)
+  {
     sum_acc = 0;
     for (i = 0; i < data_num_acc; i++)
     {

eagleye_core/navigation/src/position.cpp

@@ -75,7 +75,7 @@ void position_estimate(rtklib_msgs::RtklibNav rtklib_nav,eagleye_msgs::VelocityS
   xyz2enu(ecef_pos, ecef_base_pos, enu_pos);
 
 
-  if (position_status->tow_last == rtklib_nav.tow)
+  if (position_status->tow_last == rtklib_nav.tow || rtklib_nav.tow == 0)
   {
     gnss_status = false;
     enu_pos[0] = 0.0;
@@ -92,7 +92,7 @@ void position_estimate(rtklib_msgs::RtklibNav rtklib_nav,eagleye_msgs::VelocityS
     position_status->tow_last = rtklib_nav.tow;
   }
 
-  if (heading_interpolate_3rd.status.estimate_status == true)
+  if (heading_interpolate_3rd.status.estimate_status == true && velocity_scale_factor.status.enabled_status == true)
   {
     heading_interpolate_3rd.status.estimate_status = false; //in order to prevent being judged many times
     ++position_status->heading_estimate_status_count;
@@ -106,7 +106,7 @@ void position_estimate(rtklib_msgs::RtklibNav rtklib_nav,eagleye_msgs::VelocityS
   }
 
 
-  if (distance.distance-position_status->distance_last >= position_parameter.separation_distance && gnss_status == true)
+  if (distance.distance-position_status->distance_last >= position_parameter.separation_distance && gnss_status == true && position_status->heading_estimate_status_count > 0)
   {
 
     if (position_status->estimated_number < estimated_number_max)