visualization tools

README.md

@@ -20,4 +20,4 @@ This package contains the `rosflight_io` node, which provides the core functiona
 
 ## rosflight_utils
 
-This package contains additional supporting scripts and libraries that are not part of the core ROSflight package functionality. This package also helps support the [ros_plane](https://github.com/byu-magicc/ros_plane) and [ros_copter](https://github.com/byu-magicc/ros_copter) projects.
+This package contains additional supporting scripts and libraries that are not part of the core ROSflight package functionality, including visualization tools for the attitude estimate and magnetometer. This package also helps support the [ROSplane](https://github.com/byu-magicc/rosplane) and [ROScopter](https://github.com/byu-magicc/roscopter) projects.

rosflight_utils/CMakeLists.txt

@@ -33,3 +33,7 @@ target_link_libraries(rosflight_utils ${catkin_LIBRARIES})
 #add_executable(joy src/joy.cpp)
 #add_dependencies(joy ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 #target_link_libraries(joy ${catkin_LIBRARIES})
+
+add_executable(viz src/viz.cpp)
+add_dependencies(viz ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(viz ${catkin_LIBRARIES})

rosflight_utils/include/rosflight_utils/viz_mag.h → rosflight_utils/include/rosflight_utils/viz.h

@@ -1,47 +1,50 @@
-#ifndef VIZ_MAG_H
-#define VIZ_MAG_H
+#pragma once
+
+#include <math.h>
+#include <string>
 
 #include <ros/ros.h>
 #include <tf/tf.h>
-#include <math.h>
 #include <sensor_msgs/MagneticField.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Point.h>
+#include <rosflight_msgs/Attitude.h>
 #include <visualization_msgs/Marker.h>
 
 namespace rosflight_utils
 {
 
 
-class VizMag
+class Viz
 {
-
 public:
-
-  VizMag();
+  Viz();
 
 private:
 
   // Node handles, publishers, subscribers
   ros::NodeHandle nh_;
   ros::NodeHandle nh_private_;
 
-  // Publishers and Subscribers
+  // Magnetometer visualization pubs and subs
   ros::Subscriber mag_sub_;
   ros::Publisher mag_pub_;
   ros::Publisher pts_pub_;
 
+  // Attitude visualization pubs and subs
+  ros::Subscriber att_sub_;
+  ros::Publisher pose_pub_;
+
   // Variables
   tf::Quaternion q_att_;
   std::vector<geometry_msgs::Point> pts_list_;
   double mag_sum_;
   int mag_count_, mag_throttle_, mag_skip_;
+  std::string fixed_frame_ = "fixed_frame";
 
   // Functions
   void magCallback(const sensor_msgs::MagneticFieldConstPtr &msg);
+  void attCallback(const rosflight_msgs::AttitudeConstPtr &msg);
 };
 
-} // namespace visual_naze
-
-#endif
-
+} // namespace rosflight_utils

rosflight_utils/launch/viz_attitude.launch

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <node name="rosflight_io" pkg="rosflight" type="rosflight_io" output="screen"/>
+  <node name="viz" pkg="rosflight_utils" type="viz" output="screen"/>
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find rosflight_utils)/rviz/viz_att.rviz"/>
+</launch>

rosflight_utils/launch/viz_mag.launch

@@ -1,12 +1,6 @@
-
-<!-- This spawns an MAV and runs all nodes associated with the MAV -->
-
+<?xml version="1.0" encoding="UTF-8"?>
 <launch>
-
-  <node name="fixed_frame" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 1 map fixed_frame 10"/>
   <node name="rosflight_io" pkg="rosflight" type="rosflight_io" output="screen"/>
-  <node name="viz_mag" pkg="rosflight" type="viz_mag" output="screen"/>
-  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find rosflight)/rviz/viz_mag.rviz"/>
-
+  <node name="viz" pkg="rosflight_utils" type="viz" output="screen"/>
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find rosflight_utils)/rviz/viz_mag.rviz"/>
 </launch>
-

rosflight_utils/rviz/viz_att.rviz

@@ -0,0 +1,137 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Axes1
+        - /Pose1
+      Splitter Ratio: 0.5
+    Tree Height: 592
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.0500000007
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Axes Length: 3
+      Axes Radius: 0.100000001
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.300000012
+      Head Radius: 0.100000001
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.0500000007
+      Shape: Axes
+      Topic: /viz/attitude
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: fixed_frame
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.530398548
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.99856186
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 873
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002dffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002df000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002df000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005960000003efc0100000002fb0000000800540069006d00650100000000000005960000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000426000002df00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1430
+  X: 109
+  Y: 69

rosflight_utils/src/viz_mag.cpp → rosflight_utils/src/viz.cpp

@@ -1,9 +1,9 @@
-#include <rosflight_utils/viz_mag.h>
+#include <rosflight_utils/viz.h>
 
 namespace rosflight_utils
 {
 
-VizMag::VizMag() :
+Viz::Viz() :
   nh_private_("~")
 {
   // retrieve params
@@ -14,14 +14,17 @@ VizMag::VizMag() :
   mag_count_ = 0;
   mag_throttle_ = 0;
 
-  // Set up Publishers and Subscribers
-  mag_sub_ = nh_.subscribe("/magnetometer", 1, &VizMag::magCallback, this);
-
-  mag_pub_ = nh_.advertise<geometry_msgs::PoseStamped>( "viz/magnetometer", 1 );
-  pts_pub_ = nh_.advertise<visualization_msgs::Marker>( "viz/cloud", 1);
+  // Magnetometer visualization
+  mag_sub_ = nh_.subscribe("/magnetometer", 1, &Viz::magCallback, this);
+  mag_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("viz/magnetometer", 1);
+  pts_pub_ = nh_.advertise<visualization_msgs::Marker>("viz/cloud", 1);
+
+  // Attitude visualization
+  att_sub_ = nh_.subscribe("/attitude", 1, &Viz::attCallback, this);
+  pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("viz/attitude", 1);
 }
 
-void VizMag::magCallback(const sensor_msgs::MagneticFieldConstPtr &msg)
+void Viz::magCallback(const sensor_msgs::MagneticFieldConstPtr &msg)
 {  
   if (mag_throttle_ > mag_skip_)
   {
@@ -40,7 +43,7 @@ void VizMag::magCallback(const sensor_msgs::MagneticFieldConstPtr &msg)
     // pack data into pose message
     geometry_msgs::PoseStamped pose_msg;
     pose_msg.header = msg->header;
-    pose_msg.header.frame_id = "fixed_frame";
+    pose_msg.header.frame_id = fixed_frame_;
     pose_msg.pose.position.x = 0;
     pose_msg.pose.position.y = 0;
     pose_msg.pose.position.z = 0;
@@ -61,7 +64,7 @@ void VizMag::magCallback(const sensor_msgs::MagneticFieldConstPtr &msg)
 
     // begin packing marker message
     visualization_msgs::Marker pts_msg;
-    pts_msg.header.frame_id = "fixed_frame";
+    pts_msg.header.frame_id = fixed_frame_;
     pts_msg.header.stamp = msg->header.stamp;
     pts_msg.type = visualization_msgs::Marker::POINTS;
     pts_msg.action = visualization_msgs::Marker::ADD;
@@ -86,13 +89,23 @@ void VizMag::magCallback(const sensor_msgs::MagneticFieldConstPtr &msg)
   mag_throttle_++;
 }
 
+void Viz::attCallback(const rosflight_msgs::AttitudeConstPtr &msg)
+{
+
+  geometry_msgs::PoseStamped pose;
+  pose.header = msg->header;
+  pose.header.frame_id = fixed_frame_;
+  pose.pose.orientation = msg->attitude;
+
+  pose_pub_.publish(pose);
+}
 
-} // namespace rosflight
+} // namespace rosflight_utils
 
 int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "viz_mag_node");
-  rosflight::VizMag thing;
+  ros::init(argc, argv, "viz_node");
+  rosflight_utils::Viz viz;
   ros::spin();
   return 0;
 }