Added contact and kinematics visualization. Merged all markers into a…

… single visualization publisher.
RossHartley · Sep 24, 2018 · 8f546dd · 8f546dd
1 parent d9333a4
commit 8f546dd
Show file tree

Hide file tree

Showing 20 changed files with 42,518 additions and 22,135 deletions.
diff --git a/inekf_msgs/CMakeLists.txt b/inekf_msgs/CMakeLists.txt
@@ -12,6 +12,10 @@ find_package(
 
 add_message_files(
     FILES
+    Contact.msg
+    ContactArray.msg
+    Kinematics.msg
+    KinematicsArray.msg
     Landmark.msg
     LandmarkStamped.msg
     LandmarkArray.msg

diff --git a/inekf_msgs/msg/Contact.msg b/inekf_msgs/msg/Contact.msg
@@ -0,0 +1,3 @@
+int32 id
+bool indicator
+
diff --git a/inekf_msgs/msg/ContactArray.msg b/inekf_msgs/msg/ContactArray.msg
@@ -0,0 +1,2 @@
+Header header
+Contact[] contacts
diff --git a/inekf_msgs/msg/Kinematics.msg b/inekf_msgs/msg/Kinematics.msg
@@ -0,0 +1,2 @@
+int32 id
+geometry_msgs/PoseWithCovariance pose
diff --git a/inekf_msgs/msg/KinematicsArray.msg b/inekf_msgs/msg/KinematicsArray.msg
@@ -0,0 +1,2 @@
+Header header
+Kinematics[] frames
diff --git a/inekf_ros/CMakeLists.txt b/inekf_ros/CMakeLists.txt
@@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   sensor_msgs
   inekf_msgs
+  apriltag_msgs
   tf
 )
 
@@ -163,7 +164,8 @@ file(GLOB src_files
 )
 add_executable(inekf_node src/inekf_node.cpp ${src_files})
 add_executable(fake_imu_publisher_node tests/fake_imu_publisher.cpp)
-add_executable(fake_multi_landmark_publisher_node tests/fake_multi_landmark_publisher.cpp)
+add_executable(fake_landmark_publisher_node tests/fake_landmark_publisher.cpp)
+add_executable(fake_contact_kinematics_publisher_node tests/fake_contact_kinematics_publisher.cpp)
 add_executable(data_file_publisher_node tests/data_file_publisher.cpp)
 
 ## Rename C++ executable without prefix
@@ -186,7 +188,10 @@ target_link_libraries(inekf_node
 target_link_libraries(fake_imu_publisher_node
     ${catkin_LIBRARIES}
 )
-target_link_libraries(fake_multi_landmark_publisher_node
+target_link_libraries(fake_landmark_publisher_node
+    ${catkin_LIBRARIES}
+)
+target_link_libraries(fake_contact_kinematics_publisher_node
     ${catkin_LIBRARIES}
 )
 target_link_libraries(data_file_publisher_node

diff --git a/inekf_ros/config/prior.yaml b/inekf_ros/config/prior.yaml
@@ -4,15 +4,15 @@ prior:
   base_position: [0,0,0]
   gyroscope_bias: [0,0,0]
   accelerometer_bias: [0,0,0]
-  base_orientation_std: 0.001
+  base_orientation_std: 0.1
   base_velocity_std: 0.1
-  base_position_std: 0.01
-  gyroscope_bias_std: 0.001
-  accelerometer_bias_std: 0.01
-  landmarks: 
-    - id: 0
-      position: [1.0,1.0,0.0] 
-    - id: 1
-      position: [1.0,-1.0,0.0]
-    - id: 2
-      position: [2.0,0.0,1.0]
+  base_position_std: 0.001
+  gyroscope_bias_std: 0.2
+  accelerometer_bias_std: 0.2
+  # landmarks: 
+  #   - id: 0
+  #     position: [1.0,1.0,0.0] 
+  #   - id: 1
+  #     position: [1.0,-1.0,0.0]
+  #   - id: 2
+  #     position: [2.0,0.0,1.0]
diff --git a/inekf_ros/config/settings.yaml b/inekf_ros/config/settings.yaml
@@ -1,14 +1,31 @@
 settings:
   imu_topic: "/imu"
-  landmarks_topic: "/landmarks"
   map_frame_id : "/map"
   base_frame_id: "/imu"
-  publish_rate: 100.0
+  publish_rate: 200.0
   pose_topic: "/pose"
   state_topic: "/state"
-  publish_landmark_position_markers: true
-  landmark_position_markers_topic: "/landmark_position_markers"
-  publish_trajectory_markers: true
-  trajectory_markers_topic: "/trajectory_markers"
-  publish_landmark_measurement_markers: true
-  landmark_measurement_markers_topic: "/landmark_measurement_markers"
+  publish_visualization_markers: true
+  visualization_markers_topic: "/markers"
+  enable_landmarks: true
+  landmarks_topic: "/landmarks"
+  enable_kinematics: true
+  kinematics_topic: "/kinematics"
+  contact_topic: "/contacts"
+
+
+# settings:
+#   imu_topic: "/multisense/imu/imu_data"
+#   landmarks_topic: "/apriltag2_pub/apriltag2_tag_detections_array"
+#   map_frame_id : "/map"
+#   base_frame_id: "/multisense/accel"
+#   publish_rate: 100.0
+#   pose_topic: "/pose"
+#   state_topic: "/state"
+#   publish_landmark_position_markers: true
+#   landmark_position_markers_topic: "/landmark_position_markers"
+#   publish_trajectory_markers: true
+#   trajectory_markers_topic: "/trajectory_markers"
+#   publish_landmark_measurement_markers: true
+#   landmark_measurement_markers_topic: "/landmark_measurement_markers"
+
diff --git a/inekf_ros/data/sim_data_2.txt b/inekf_ros/data/sim_data_2.txt
diff --git a/inekf_ros/include/InEKF_ROS.h b/inekf_ros/include/InEKF_ROS.h
@@ -16,6 +16,8 @@
 #include "Queue.h"
 #include "inekf_msgs/State.h"
 #include "visualization_msgs/MarkerArray.h"
+#include "apriltag_msgs/AprilTagDetectionArray.h"
+#include <mutex>
 
 #define MAX_QUEUE_SIZE 100
 
@@ -30,25 +32,31 @@ class InEKF_ROS {
         inekf::InEKF filter_;
         ros::Subscriber imu_sub_;
         ros::Subscriber landmarks_sub_;
+        ros::Subscriber kinematics_sub_;
+        ros::Subscriber contact_sub_;
         std::thread filtering_thread_;
         std::thread output_thread_;
         Queue<std::shared_ptr<Measurement>> m_queue_;
 
         std::string imu_frame_id_;
         std::string map_frame_id_;
+        bool publish_visualization_markers_;
+        ros::Publisher visualization_pub_;
+        bool enable_landmarks_;
         tf::StampedTransform camera_to_imu_transform_;
-        bool publish_landmark_measurement_markers_;
-        bool publish_landmark_position_markers_;
-        bool publish_trajectory_markers_;
-        ros::Publisher landmark_measurement_vis_pub_;
-
+        bool enable_kinematics_;
 
         void subscribe();
         void mainFilteringThread();
         void outputPublishingThread();
         void imuCallback(const sensor_msgs::Imu::ConstPtr& msg); 
         void landmarkCallback(const inekf_msgs::LandmarkArray::ConstPtr& msg);
+        void aprilTagCallback(const apriltag_msgs::AprilTagDetectionArray::ConstPtr& msg);
+        void kinematicsCallback(const inekf_msgs::KinematicsArray::ConstPtr& msg);
+        void contactCallback(const inekf_msgs::ContactArray::ConstPtr& msg);
+
         void publishLandmarkMeasurementMarkers(std::shared_ptr<LandmarkMeasurement> ptr);
+        void publishKinematicMeasurementMarkers(std::shared_ptr<KinematicMeasurement> ptr);
 };
 
 #endif 
diff --git a/inekf_ros/include/Measurement.h b/inekf_ros/include/Measurement.h
@@ -4,12 +4,13 @@
 #include <string>
 #include "ros/ros.h"
 #include "sensor_msgs/Imu.h"
-#include "inekf_msgs/LandmarkStamped.h"
+#include "inekf_msgs/ContactArray.h"
+#include "inekf_msgs/KinematicsArray.h"
 #include "inekf_msgs/LandmarkArray.h"
 #include "InEKF.h"
 #include "tf/transform_listener.h"
 
-enum MeasurementType {EMPTY, IMU, LANDMARK};
+enum MeasurementType {EMPTY, IMU, LANDMARK, KINEMATIC, CONTACT};
 
 class Measurement {
 
@@ -54,4 +55,30 @@ class LandmarkMeasurement : public Measurement {
         inekf::vectorPairIntVector3d data_;
 };
 
+class ContactMeasurement : public Measurement {
+
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        ContactMeasurement(const inekf_msgs::ContactArray::ConstPtr& msg);
+        std::vector<std::pair<int,bool>> getData();
+        //friend std::ostream& operator<<(std::ostream& os, const Measurement& m);  
+
+    private:
+        std::vector<std::pair<int,bool>> data_;
+};
+
+
+class KinematicMeasurement : public Measurement {
+
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        KinematicMeasurement(const inekf_msgs::KinematicsArray::ConstPtr& msg);
+        inekf::vectorTupleIntMatrix4dMatrix6d getData();
+        //friend std::ostream& operator<<(std::ostream& os, const Measurement& m);  
+
+    private:
+        inekf::vectorTupleIntMatrix4dMatrix6d data_;
+};
+
+
 #endif 
diff --git a/inekf_ros/launch/data_file_test.launch b/inekf_ros/launch/data_file_test.launch
@@ -7,10 +7,18 @@
     <rosparam file="$(find inekf_ros)/config/prior.yaml" command="load" />
   </node>
 
+    <!-- Apriltag node -->
+	<!--<node pkg="apriltag2ROS" type="apriltag2_pub" name="apriltag2_pub" />-->
+
     <!-- Create data publisher-->
-  <node name="data_file_publisher" pkg="inekf_ros" type="data_file_publisher_node">
-    <param name="data_file" type="string" value="$(find inekf_ros)/data/sim_data_2.txt" />
-  </node>
+    <node name="data_file_publisher" pkg="inekf_ros" type="data_file_publisher_node">
+      <param name="data_file" type="string" value="$(find inekf_ros)/data/sim_data_2.txt" />
+    </node>
+
+
+
+  <!--<node pkg="rosbag" type="play" name="player" args="clock /mnt/c/Users/Ross\ Hartley/Documents/GitHub/personal/bagfiles/2018-09-20-11-03-09.bag"/>-->
+
 
   <node type="rviz" name="rviz" pkg="rviz" args="-d $(find inekf_ros)/rviz/config.rviz" />
 

diff --git a/inekf_ros/launch/fake_imu_kinematics.launch b/inekf_ros/launch/fake_imu_kinematics.launch
@@ -0,0 +1,33 @@
+<launch>
+
+  <!-- Create fake IMU data publisher-->
+  <node name="fake_imu_publisher" pkg="inekf_ros" type="fake_imu_publisher_node">
+    <rosparam file="$(find inekf_ros)/config/fake_data_noise.yaml" command="load" />
+    <param name="rate" value="200" />
+  </node>
+
+  <!-- Create fake contact and kinematics data publisher-->
+  <node name="fake_contact_kinematics_publisher" pkg="inekf_ros" type="fake_contact_kinematics_publisher_node">
+    <rosparam file="$(find inekf_ros)/config/fake_data_noise.yaml" command="load" />
+    <param name="rate" value="200" />
+  </node>
+
+  <!-- Create fake landmark data publisher-->
+  <node name="fake_landmark_publisher" pkg="inekf_ros" type="fake_landmark_publisher_node">
+    <rosparam file="$(find inekf_ros)/config/fake_data_noise.yaml" command="load" />
+    <param name="rate" value="20" />
+  </node>
+
+  <!-- Run Invariant EKF -->
+  <node name="inekf" pkg="inekf_ros" type="inekf_node" output="screen">
+    <rosparam file="$(find inekf_ros)/config/settings.yaml" command="load" />
+    <rosparam file="$(find inekf_ros)/config/noise.yaml" command="load" />
+    <rosparam file="$(find inekf_ros)/config/prior.yaml" command="load" />
+  </node>
+
+  <!-- roscd removes spaces in path name which casues the following to not work -->
+  <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d `rospack find inekf_ros`/rviz/config.rviz" /> -->
+  <node type="rviz" name="rviz" pkg="rviz" args="-d /mnt/c/Users/Ross\ Hartley/Documents/GitHub/personal/catkin_ws/src/invariant-ekf_ros/inekf_ros/rviz/config.rviz" />
+
+
+</launch>
diff --git a/inekf_ros/launch/fake_imu_landmark_test.launch b/inekf_ros/launch/fake_imu_landmark_test.launch
@@ -7,7 +7,7 @@
   </node>
 
   <!-- Create fake landmark data publisher-->
-  <node name="fake_landmark_publisher" pkg="inekf_ros" type="fake_multi_landmark_publisher_node">
+  <node name="fake_landmark_publisher" pkg="inekf_ros" type="fake_landmark_publisher_node">
     <rosparam file="$(find inekf_ros)/config/fake_data_noise.yaml" command="load" />
     <param name="rate" value="20" />
   </node>

diff --git a/inekf_ros/package.xml b/inekf_ros/package.xml
@@ -53,6 +53,7 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>inekf_msgs</depend>
+  <depend>apriltag_msgs</depend>
   <depend>tf</depend>
   <buildtool_depend>catkin</buildtool_depend>
 

diff --git a/inekf_ros/rviz/config.rviz b/inekf_ros/rviz/config.rviz
@@ -6,6 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /MarkerArray1
+        - /MarkerArray1/Namespaces1
       Splitter Ratio: 0.5
     Tree Height: 542
   - Class: rviz/Selection
@@ -68,28 +70,16 @@ Visualization Manager:
             {}
       Update Interval: 0
       Value: true
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /trajectory_markers
-      Name: Marker
-      Namespaces:
-        trajectory: true
-      Queue Size: 100
-      Value: true
-    - Class: rviz/Marker
-      Enabled: true
-      Marker Topic: /landmark_measurement_markers
-      Name: Marker
-      Namespaces:
-        trajectory: true
-      Queue Size: 100
-      Value: true
     - Class: rviz/MarkerArray
       Enabled: true
-      Marker Topic: /landmark_position_markers
+      Marker Topic: /markers
       Name: MarkerArray
       Namespaces:
+        contacts: true
+        estimated_contacts: true
+        estimated_landmarks: true
         landmarks: true
+        trajectory: true
       Queue Size: 100
       Value: true
   Enabled: true
@@ -132,10 +122,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.355397969
+      Pitch: 0.520397902
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.47540188
+      Yaw: 3.35040116
     Saved: ~
 Window Geometry:
   Displays:
@@ -153,5 +143,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1536
-  X: 101
-  Y: 52
+  X: 125
+  Y: 145
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,2 @@
		int32 id
		geometry_msgs/PoseWithCovariance pose