vis color opt

include/sweet_rviz_panel/sweet_rviz_panel.hpp

@@ -43,8 +43,8 @@ private:
   void poseCallback(const geometry_msgs::msg::Pose::SharedPtr msg);
   void velCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
   void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
-  void calculateErrors();
+  void calculateData();
-  void updateErrorPlots();
+  void updateDataPlot();
   void initPlots();
 
   QLabel *x_label_;
@@ -59,6 +59,7 @@ private:
   QTimer *timer_;
   QCustomPlot *lateral_plot_;
   QCustomPlot *heading_plot_;
+  QCustomPlot *yaw_cmd_plot_;
   rclcpp::Node::SharedPtr node_;
   std::shared_ptr<
   rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
@@ -74,13 +75,16 @@ private:
   QVector<double> error_time_;
   QVector<double> lateral_errors_;
   QVector<double> heading_errors_;
+  QVector<double> yaw_cmd_;
   QElapsedTimer error_timer_;
-  static constexpr int MAX_ERROR_POINTS = 1000;
+  static constexpr int MAX_ERROR_POINTS = 110;
-
+  static constexpr int TIME_LIMIT = 10;
   double search_distance_ = 10.0;
   int start_index_=0;
   int forward_index_ = 100;
 
+  double cur_vel_cmd_ = 0.0;
+  double cur_yaw_cmd_ = 0.0;
   bool running_;
 };
 

src/RizConfig.rviz

@@ -0,0 +1,313 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1
+        - /Grid1/Offset1
+        - /TF1
+        - /TF1/Frames1
+        - /Axes1
+      Splitter Ratio: 0.4790419042110443
+    Tree Height: 774
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+      - /Back vision1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: sweet_rviz_panel/SweetRvizPanel
+    Name: SweetRvizPanel
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.699999988079071
+      Cell Size: 2
+      Class: rviz_default_plugins/Grid
+      Color: 46; 52; 54
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Billboards
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 200
+      Reference Frame: odom
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: PlanningPath
+      Namespaces:
+        path: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rviz_planning_path
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: false
+      Name: VehiclePose
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rviz_vehicle_pose
+      Value: false
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: true
+      Description File: /home/sweetai/webots_simulation/src/sweet_webots_robot/urdf/rviz_robot.urdf
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: SweetRobot
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: HistoryPosePath
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rviz_history_traj
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: false
+      Name: TextInfo
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rviz_text_info
+      Value: false
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 3
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        odom:
+          base_link:
+            {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.05000000074505806
+      Reference Frame: odom
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 211; 215; 207
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+    - Class: nav2_rviz_plugins/GoalTool
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/ThirdPersonFollower
+      Distance: 11.149712562561035
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.0536713600158691
+        Y: -0.3854414224624634
+        Z: -0.37999600172042847
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.10000000149011612
+      Pitch: 0.5697959065437317
+      Target Frame: base_link
+      Value: ThirdPersonFollower (rviz_default_plugins)
+      Yaw: 3.124208688735962
+    Saved:
+      - Class: rviz_default_plugins/ThirdPersonFollower
+        Distance: 10.54976749420166
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 1.0536713600158691
+          Y: -0.3854414224624634
+          Z: -0.37999600172042847
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: Back vision
+        Near Clip Distance: 0.10000000149011612
+        Pitch: 0.4897962808609009
+        Target Frame: base_link
+        Value: ThirdPersonFollower (rviz_default_plugins)
+        Yaw: 3.124211072921753
+      - Class: rviz_default_plugins/ThirdPersonFollower
+        Distance: 9.699112892150879
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 1.0536713600158691
+          Y: -0.3854414224624634
+          Z: -0.37999600172042847
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: Third vision
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.2047973871231079
+        Target Frame: base_link
+        Value: ThirdPersonFollower (rviz_default_plugins)
+        Yaw: 5.914216041564941
+      - Class: rviz_default_plugins/ThirdPersonFollower
+        Distance: 8.171782493591309
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 1.0536713600158691
+          Y: -0.3854414224624634
+          Z: -0.37999600172042847
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: Front vision
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 0.30479729175567627
+        Target Frame: base_link
+        Value: ThirdPersonFollower (rviz_default_plugins)
+        Yaw: 0.021031875163316727
+      - Class: rviz_default_plugins/FPS
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Invert Z Axis: false
+        Name: Top Vision
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 1.2897965908050537
+        Position:
+          X: 0.6246874332427979
+          Y: -2.938462495803833
+          Z: 28.4678955078125
+        Target Frame: base_link
+        Value: FPS (rviz_default_plugins)
+        Yaw: 1.364442229270935
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1926
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002d8000006eafc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a00560069006500770073010000006e000002e00000013200fffffffb000000100044006900730070006c006100790073010000035a000003fe0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000035f000006eafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00530077006500650074005200760069007a00500061006e0065006c010000006e000006ea000002aa00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000c800000022cfc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000631000006ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  SweetRvizPanel:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 3200
+  X: 0
+  Y: 0

src/sweet_rviz_panel.cpp

@@ -58,11 +58,13 @@ SweetRvizPanel::SweetRvizPanel(QWidget *parent)
 
   lateral_plot_ = new QCustomPlot(this);
   heading_plot_ = new QCustomPlot(this);
+  yaw_cmd_plot_ = new QCustomPlot(this);
 
   main_layout->addWidget(pose_box);
   main_layout->addWidget(vel_cmd_box);
   main_layout->addWidget(lateral_plot_,1);
   main_layout->addWidget(heading_plot_,1);
+  main_layout->addWidget(yaw_cmd_plot_,1);
   //main_layout->addLayout(button_layout);
   main_layout->addWidget(status_label_);
   main_layout->addStretch();
@@ -83,81 +85,63 @@ SweetRvizPanel::SweetRvizPanel(QWidget *parent)
 
   error_timer_.start();
   timer_ = new QTimer(this);
-  connect(
+  connect(timer_,&QTimer::timeout,
-    timer_,
+          this,&SweetRvizPanel::calculateData);
-    &QTimer::timeout,
+  timer_->start(100);
-    this,
-    &SweetRvizPanel::calculateErrors);
-
-timer_->start(100);
 }
 void SweetRvizPanel::initPlots()
 {
+  // data color
+  QColor data_color(10,10,250);
   lateral_plot_->addGraph();
-
+  lateral_plot_->graph(0)->setName("Lateral Error");
-  lateral_plot_->graph(0)->setName(
+  lateral_plot_->xAxis->setLabel("Time (s)");
-    "Lateral Error");
+  lateral_plot_->yAxis->setLabel("Lateral Error (m)");
-
+  lateral_plot_->xAxis->setRange(0,TIME_LIMIT);
-  lateral_plot_->xAxis->setLabel(
+  lateral_plot_->yAxis->setRange(-1,1);
-    "Time (s)");
+  QPen lateral_pen(data_color);
-
+  lateral_pen.setWidth(5);
-  lateral_plot_->yAxis->setLabel(
-    "Lateral Error (m)");
-
-  lateral_plot_->xAxis->setRange(
-    0,
-    30);
-
-  lateral_plot_->yAxis->setRange(
-    -1,
-    1);
-  QPen lateral_pen(Qt::blue);
-  lateral_pen.setWidth(3);
   lateral_plot_->graph(0)->setPen(lateral_pen);
   lateral_plot_->legend->setVisible(true);
 
-
   heading_plot_->addGraph();
-
+  heading_plot_->graph(0)->setName("Heading Error");
-  heading_plot_->graph(0)->setName(
+  heading_plot_->xAxis->setLabel("Time (s)");
-    "Heading Error");
+  heading_plot_->yAxis->setLabel("Heading Error (rad)");
-
+  heading_plot_->xAxis->setRange( 0,TIME_LIMIT);
-  heading_plot_->xAxis->setLabel(
+  heading_plot_->yAxis->setRange(-0.5,0.5);
-    "Time (s)");
+  QPen heading_pen(data_color);
-
+  heading_pen.setWidth(5);
-  heading_plot_->yAxis->setLabel(
-    "Heading Error (rad)");
-
-  heading_plot_->xAxis->setRange(
-    0,
-    30);
-
-  heading_plot_->yAxis->setRange(
-    -0.5,
-    0.5);
-  QPen heading_pen(Qt::blue);
-  heading_pen.setWidth(3);
   heading_plot_->graph(0)->setPen(heading_pen);
   heading_plot_->legend->setVisible(true);
   
+  yaw_cmd_plot_->addGraph();
+  yaw_cmd_plot_->graph(0)->setName("Yaw Command");
+  yaw_cmd_plot_->xAxis->setLabel("Time (s)");
+  yaw_cmd_plot_->yAxis->setLabel("Yaw Command (rad/s)");
+  yaw_cmd_plot_->xAxis->setRange(0,TIME_LIMIT);
+  yaw_cmd_plot_->yAxis->setRange(-1,1);
+  QPen yaw_cmd_pen(data_color);
+  yaw_cmd_pen.setWidth(5);
+  yaw_cmd_plot_->graph(0)->setPen(yaw_cmd_pen);
+  yaw_cmd_plot_->legend->setVisible(true);
+
   // bg color
   QColor bg_color(240, 240, 240);
 
   lateral_plot_->setBackground(bg_color);
   heading_plot_->setBackground(bg_color);
+  yaw_cmd_plot_->setBackground(bg_color);
 
   lateral_plot_->axisRect()->setBackground(bg_color);
   heading_plot_->axisRect()->setBackground(bg_color);
+  yaw_cmd_plot_->axisRect()->setBackground(bg_color);
 }
 void SweetRvizPanel::onInitialize()
 {
   node_ = std::make_shared<rclcpp::Node>("sweet_rviz_panel");
   node_ptr_= getDisplayContext()->getRosNodeAbstraction().lock();
   if (!node_ptr_) {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("SweetRvizPanel"),
-      "Failed to get RViz ROS node");
-
     return;
   }
   auto node = node_ptr_->get_raw_node();
@@ -225,6 +209,9 @@ void SweetRvizPanel::velCmdCallback(
 
   yaw_cmd_label_->setText(
     QString("Yaw Command: %1 rad/s").arg(msg->angular.z, 0, 'f', 3));
+
+  cur_vel_cmd_ = msg->linear.x;
+  cur_yaw_cmd_ = msg->angular.z;
 }
 void SweetRvizPanel::pathCallback(
   const nav_msgs::msg::Path::SharedPtr msg)
@@ -233,10 +220,8 @@ void SweetRvizPanel::pathCallback(
     return;
   current_path_ = *msg;
   path_received_ = true;
-
-  calculateErrors();
 }
-void SweetRvizPanel::calculateErrors()
+void SweetRvizPanel::calculateData()
 {
   if (!pose_received_ || !path_received_)
     return;
@@ -291,11 +276,9 @@ void SweetRvizPanel::calculateErrors()
   const double dy =current_y_ - target.position.y;
 
   const double lateral_error =
-    -std::sin(target_yaw) * dx
+    -std::sin(target_yaw) * dx + std::cos(target_yaw) * dy;
-    + std::cos(target_yaw) * dy;
 
-  double heading_error =
+  double heading_error = current_yaw_ - target_yaw;
-    current_yaw_ - target_yaw;
 
   while (heading_error > M_PI)
     heading_error -= 2.0 * M_PI;
@@ -309,26 +292,21 @@ void SweetRvizPanel::calculateErrors()
   error_time_.push_back(t);
   lateral_errors_.push_back(lateral_error);
   heading_errors_.push_back(heading_error);
+  yaw_cmd_.push_back(cur_yaw_cmd_);
 
   if (error_time_.size() > MAX_ERROR_POINTS)
   {
     error_time_.remove(0);
     lateral_errors_.remove(0);
     heading_errors_.remove(0);
+    yaw_cmd_.remove(0);
   }
-
+  updateDataPlot();
-  updateErrorPlots();
 }
 
-void SweetRvizPanel::updateErrorPlots()
+void SweetRvizPanel::updateDataPlot()
 {
-
   if (!pose_received_) {
-    RCLCPP_INFO_THROTTLE(
-      node_->get_logger(),
-      *node_->get_clock(),
-      1000,
-      "NO POSE");
     return;
   }
   if (error_time_.isEmpty())
@@ -336,21 +314,21 @@ void SweetRvizPanel::updateErrorPlots()
 
   lateral_plot_->graph(0)->setData(error_time_,lateral_errors_);
   heading_plot_->graph(0)->setData(error_time_,heading_errors_);
+  yaw_cmd_plot_->graph(0)->setData(error_time_,yaw_cmd_);
 
   const double t_end =error_time_.last();
-  const double t_start = std::max(0.0, t_end - 30.0);
+  const double t_start = std::max(0.0, t_end - TIME_LIMIT);
 
   lateral_plot_->xAxis->setRange(t_start,std::max(t_end, t_start + 1.0));
   heading_plot_->xAxis->setRange(t_start,std::max(t_end, t_start + 1.0));
+  yaw_cmd_plot_->xAxis->setRange(t_start,std::max(t_end, t_start + 1.0));
 
   double max_lateral = 0.1;
-
   for (double e : lateral_errors_)
   {
     if (std::abs(e) > max_lateral)
       max_lateral = std::abs(e);
   }
-
   lateral_plot_->yAxis->setRange(-max_lateral * 1.2,max_lateral * 1.2);
 
   double max_heading = 0.01;
@@ -359,13 +337,19 @@ void SweetRvizPanel::updateErrorPlots()
     if (std::abs(e) > max_heading)
       max_heading = std::abs(e);
   }
+  heading_plot_->yAxis->setRange(-max_heading * 1.2,max_heading * 1.2);
 
-  heading_plot_->yAxis->setRange(
+  double max_yaw = 0.01;
-    -max_heading * 1.2,
+  for (double e : yaw_cmd_)
-    max_heading * 1.2);
+  {
+    if (std::abs(e) > max_yaw)
+      max_yaw = std::abs(e);
+  }
+  yaw_cmd_plot_->yAxis->setRange(-max_yaw * 1.2,max_yaw * 1.2);
 
   lateral_plot_->replot(QCustomPlot::rpQueuedReplot);
   heading_plot_->replot(QCustomPlot::rpQueuedReplot);
+  yaw_cmd_plot_->replot(QCustomPlot::rpQueuedReplot);
 }
 
 void SweetRvizPanel::onStart()