bug fixes and reorganisation

.gitignore

@@ -3,3 +3,4 @@
 Changelog.md
 bitbucket-pipelines.yml
 *CHANGELOG.rst
+*.pyc

vrx_gazebo/launch/sandisland.launch

@@ -28,10 +28,10 @@
 
   <!-- Do you want to enable sensors? -->
   <arg name="camera_enabled"       default="false" />
-  <arg name="gps_enabled"          default="false" />
-  <arg name="imu_enabled"          default="false" />
+  <arg name="gps_enabled"          default="true" />
+  <arg name="imu_enabled"          default="true" />
   <arg name="lidar_enabled"        default="false" />
-  <arg name="ground_truth_enabled" default="false" />
+  <arg name="ground_truth_enabled" default="true" />
 
   <!-- Start Gazebo with the world file -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">

wamv_controls/sensor_fusion/CMakeLists.txt

@@ -7,8 +7,8 @@ if (NOT CMAKE_BUILD_TYPE)
       FORCE)
 endif (NOT CMAKE_BUILD_TYPE)
 
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -Wall")
-set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -std=c++11 -O3 -s")
+set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++14 -Wall")
+set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -std=c++14 -O3 -s")
 
 set(SRC_DIR "src")
 set(INCLUDE_DIR "include")
@@ -34,6 +34,9 @@ add_service_files(
         ResetIMU.srv
 )
 
+# Python scripts setup
+catkin_python_setup()
+
 # Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
@@ -65,4 +68,4 @@ include_directories(
 
 add_executable(${PROJECT_NAME}_node ${SRC_DIR}/nav_node.cpp ${FILES})
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})
-add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencpp)
+add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencpp)

wamv_controls/sensor_fusion/include/gps_data.h

@@ -7,12 +7,12 @@
 
 namespace navigation{
 
-    class GPSData: public NavigationDevice 
+    class GPSData: public NavigationDevice
     {
         typedef void (GPSData::*IntegrationMethodT) (const double &);
-    
+
     public:
-    
+
         enum IntegrationMethodType
         {
             StdMethod  = 0,

wamv_controls/sensor_fusion/include/nav_main.h

@@ -1,20 +1,24 @@
 #ifndef NAV_MAIN_H_
 #define NAV_MAIN_H_
 
-#include <ros/ros.h>
+#include <iostream>
+#include <chrono>
+#include <thread>
 #include <memory>
+
+#include <ros/ros.h>
 #include <eigen3/Eigen/Geometry>
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <sensor_msgs/Imu.h>
 #include <nav_msgs/Odometry.h>
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_datatypes.h>
+#include "sensor_fusion/ResetIMU.h"
 
 #include "gps_data.h"
 #include "imu_data.h"
 #include "ekf.h"
-#include <sensor_fusion/ResetIMU.h>
 
 namespace navigation
 {
@@ -65,4 +69,4 @@ class NavigationNode
 };
 
 } // namespace navigation
-#endif //NAVIGATION_NODE_H_
+#endif //NAVIGATION_NODE_H_

wamv_controls/sensor_fusion/launch/odom.launch

@@ -1,5 +1,5 @@
 <launch>
-    <node pkg="vrx_gazebo" name="velocity_node" type="velocity.py" output="screen" />
+    <node pkg="sensor_fusion" name="velocity_node" type="velocity.py" output="screen" />
     <node pkg="sensor_fusion" name="odometry_publisher" type="sensor_fusion_node" output="screen" />
 
     <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output="screen">

wamv_controls/sensor_fusion/setup.py

@@ -0,0 +1,10 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(packages=['sensor_fusion'],
+                                      package_dir={'': 'src'},)
+
+setup(**setup_args)

wamv_controls/sensor_fusion/src/gps_data.cpp

@@ -51,7 +51,8 @@ namespace navigation{
         positionIncrement_ << gps_twist_.x * dt_sec, gps_twist_.y * dt_sec, gps_twist_.z * dt_sec;
         historyPositionIncrement_.block<3,3>(0, 1) = historyPositionIncrement_.block<3,3>(0, 0);
         historyPositionIncrement_.col(0) = positionIncrement_;
-        positionIncrement_ = (1.0 / 6.0) * (historyPositionIncrement_.col(0) + 2 * historyPositionIncrement_.col(1) + 2 * historyPositionIncrement_.col(2) + historyPositionIncrement_.col(3));
+        positionIncrement_ = (1.0 / 6.0) * (historyPositionIncrement_.col(0) + 2 * historyPositionIncrement_.col(1) +
+                                2 * historyPositionIncrement_.col(2) + historyPositionIncrement_.col(3));
     }
 
     Eigen::Vector3d GPSData::GetVelocityXYZ()
@@ -60,4 +61,4 @@ namespace navigation{
         twist << gps_twist_.x, gps_twist_.y, gps_twist_.z;
         return twist;
     }
-}
+}

wamv_controls/sensor_fusion/src/nav_main.cpp

@@ -2,7 +2,7 @@
 
 namespace navigation {
 
-NavigationNode::NavigationNode(const ros::NodeHandlePtr &nh) : 
+NavigationNode::NavigationNode(const ros::NodeHandlePtr &nh) :
     nh_(nh), quaternion_(0.0, 0.0, 0.0, 0.0) {
     gpsTwistSubscriber_ = nh_->subscribe("/gps/velocity", 100, &GPSData::GPSTwistCallback, &gpsData_);
     imuSubscriber_ = nh_->subscribe("/imu/data", 100, &IMUData::IMUMsgCallback, &imuData_);
@@ -11,7 +11,7 @@ NavigationNode::NavigationNode(const ros::NodeHandlePtr &nh) :
     odom_pub_ = nh_->advertise<nav_msgs::Odometry>("/wamv/pose_gt/relay", 100);
 
     reset_imu_ = nh_->advertiseService("/wamv/reset_imu", &navigation::NavigationNode::resetCB, this);
-    
+
     position_ = Eigen::Vector3d::Zero();
     local_position_ = Eigen::Vector3d::Zero();
     incrementPosition_ = Eigen::Vector3d::Zero();
@@ -30,12 +30,13 @@ NavigationNode::~NavigationNode() {
 }
 
 void NavigationNode::Spin() {
+    using namespace std::chrono_literals;
     ros::Rate loop_rate(15); // 100 hz
     while (ros::ok())
     {
         ros::spinOnce();
         ProcessCartesianPose();
-        loop_rate.sleep();
+        std::this_thread::sleep_for(0.05s);
     }
 }
 
@@ -55,7 +56,7 @@ Eigen::Vector3d toEulerAngle(const Eigen::Quaterniond& q) {
 
 	// yaw (z-axis rotation)
 	double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
-	double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());  
+	double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
 	yaw = atan2(siny_cosp, cosy_cosp);
 
     Eigen::Vector3d euler;
@@ -81,7 +82,7 @@ bool NavigationNode::resetCB (sensor_fusion::ResetIMU::Request& req,
 
     res.success = true;
     ROS_INFO("Service completed");
-    return true; 
+    return true;
 }
 
 void NavigationNode::correct_orientation (Eigen::Quaterniond& q) {
@@ -167,8 +168,8 @@ void NavigationNode::FillPoseMsg(Eigen::Vector3d &position,
     msg.pose.pose.orientation.w = quaternion.w();
 }
 
-void NavigationNode::FillTwistMsg(Eigen::Vector3d &linear_velocity, 
-                                  Eigen::Vector3d &angular_velocity, 
+void NavigationNode::FillTwistMsg(Eigen::Vector3d &linear_velocity,
+                                  Eigen::Vector3d &angular_velocity,
                                   nav_msgs::Odometry &msg) {
     msg.twist.twist.linear.x = linear_velocity.x();
     msg.twist.twist.linear.y = linear_velocity.y();

wamv_controls/sensor_fusion/src/nav_node.cpp

@@ -3,9 +3,9 @@
 
 int main(int argc, char** argv) {
   ros::init(argc, argv, "navigation_node");
-
+  ros::Time::init();
   ros::NodeHandlePtr nh(new ros::NodeHandle("~"));
   navigation::NavigationNode navigation_node(nh);
   ROS_INFO("Started the navigation_node handler");
   navigation_node.Spin();
-}
+}

vrx_gazebo/nodes/velocity.py → wamv_controls/sensor_fusion/src/velocity.py

@@ -6,6 +6,7 @@
 from sensor_msgs.msg import Imu
 from geometry_msgs.msg import Vector3Stamped, Vector3
 import tf.transformations as trans
+import time
 
 imu_data = Imu()
 gps_vel = Vector3Stamped()
@@ -24,7 +25,7 @@ def vel_fix(vel):
     org_vel = numpy.array([vel.x, vel.y, vel.z])
     trans_vel = trans.quaternion_matrix(q).transpose()[0:3,0:3].dot(org_vel)
     corr_vel = Vector3(trans_vel[0], trans_vel[1], trans_vel[2])
-    print ("corrected vel: \n{}".format(corr_vel))
+    # print ("corrected vel: \n{}".format(corr_vel))
     return corr_vel
 
 if __name__ == '__main__':
@@ -44,4 +45,4 @@ def vel_fix(vel):
 
         correct_velocity = vel_fix(correct_velocity)
         vel_pub.publish(correct_velocity)
-        rospy.sleep(0.05)
+        time.sleep(0.05)

wamv_master/launch/master.launch

@@ -1,8 +1,8 @@
 <launch>
     <include file="$(find vrx_gazebo)/launch/vrx.launch"/>
-    <!---->include file="$(find sensor_fusion)/launch/odom.launch"/-->
+    <!-- <include file="$(find sensor_fusion)/launch/odom.launch" /> -->
     <include file="$(find wamv_gazebo)/launch/localization_example.launch"/>
     <include file="$(find wamv_gazebo)/launch/rviz_vrx.launch"/>
 
     <include file="$(find cascaded_pid_controller)/launch/velocity_hold.launch"/>
-</launch>
+</launch>

wamv_vision/CMakeLists.txt → wamv_perception/wamv_vision/CMakeLists.txt

@@ -23,7 +23,7 @@ find_package(Boost REQUIRED COMPONENTS system)
 find_package(OpenCV REQUIRED)
 generate_messages (
     DEPENDENCIES
-    std_msgs   
+    std_msgs
 )