[ready] Websocket server for negotiation visualizer

README.md

@@ -73,6 +73,10 @@ To start the websocket server,
 
 The default port_number of the websocet server is `8006`. 
 
+To test negotiation status trajectories, we can setup negotiations to be retained. Add `-history 999` to the following line in `server.xml`:
+
+``` <node pkg="rmf_schedule_visualizer" exec="schedule_visualizer" args="-p $(var port) -history 999"> ```
+
 ### Sample Client Requests
 
 #### Server Time

rmf_schedule_visualizer/CMakeLists.txt

@@ -36,7 +36,10 @@ endif()
 #===============================================================================
 #file(GLOB_RECURSE visualizer_srcs "src/*.cpp")
 #add_executable(schedule_visualizer ${visualizer_srcs})
-add_executable(schedule_visualizer src/Server.cpp src/VisualizerData.cpp src/main.cpp)
+add_executable(schedule_visualizer 
+  src/Server.cpp
+  src/VisualizerData.cpp
+  src/main.cpp)
 
 target_link_libraries(schedule_visualizer
   PUBLIC

rmf_schedule_visualizer/src/Server.cpp

@@ -58,6 +58,59 @@ Server::Server(uint16_t port, VisualizerDataNode& visualizer_data_node)
   _server.set_close_handler(bind(&Server::on_close, this, _1));
   _server.set_message_handler(bind(&Server::on_message, this, _1, _2));
   _is_initialized = true;
+
+  // TODO: Eliminate undefined behavior from this implementation
+  // https://github.com/osrf/rmf_schedule_visualizer/pull/69#discussion_r476166450
+
+  //set up callbacks for negotiations
+  auto status_update_cb = [this](
+    uint64_t conflict_version,
+    rmf_traffic::schedule::Negotiation::Table::ViewerPtr table_view)
+    {
+      RCLCPP_DEBUG(_visualizer_data_node.get_logger(),
+        "======== conflict callback version: %llu! ==========",
+        conflict_version);
+
+      nlohmann::json negotiation_json;
+      negotiation_json["type"] = "negotiation_status";
+      negotiation_json["conflict_version"] = conflict_version;
+      negotiation_json["participant_id"] = table_view->participant_id();
+      negotiation_json["participant_name"] =
+        table_view->get_description(table_view->participant_id())->name();
+      negotiation_json["defunct"] = table_view->defunct();
+      negotiation_json["rejected"] = table_view->rejected();
+      negotiation_json["forfeited"] = table_view->forfeited();
+
+      auto versioned_sequence = table_view->sequence();
+      for (auto versionedkey : versioned_sequence)
+        negotiation_json["sequence"].push_back(versionedkey.participant);
+
+      std::string conflict_str = negotiation_json.dump();
+      for (auto connection : _connections)
+        _server.send(connection, conflict_str,
+          websocketpp::frame::opcode::text);
+    };
+  _visualizer_data_node._negotiation->on_status_update(std::move(
+      status_update_cb));
+
+  auto conclusion_cb = [this](
+    uint64_t conflict_version, bool resolved)
+    {
+      RCLCPP_DEBUG(_visualizer_data_node.get_logger(),
+        "======== conflict concluded: %llu resolved: %d ==========",
+        conflict_version, resolved ? 1 : 0);
+
+      nlohmann::json json_msg;
+      json_msg["type"] = "negotiation_conclusion";
+      json_msg["conflict_version"] = conflict_version;
+      json_msg["resolved"] = resolved;
+
+      std::string json_str = json_msg.dump();
+      for (auto connection : _connections)
+        _server.send(connection, json_str, websocketpp::frame::opcode::text);
+    };
+  _visualizer_data_node._negotiation->on_conclusion(std::move(conclusion_cb));
+
 }
 
 void Server::on_open(connection_hdl hdl)
@@ -149,7 +202,9 @@ bool Server::parse_request(server::message_ptr msg, std::string& response)
       auto elements = _visualizer_data_node.get_elements(request_param);
 
       bool trim = j_param["trim"];
-      response = parse_trajectories(elements, trim, request_param);
+      response = parse_trajectories("trajectory",
+          _visualizer_data_node.get_server_conflicts(), elements, trim,
+          request_param);
 
       return true;
 
@@ -166,6 +221,30 @@ bool Server::parse_request(server::message_ptr msg, std::string& response)
       return true;
     }
 
+    else if (j["request"] == "negotiation_trajectory")
+    {
+      RCLCPP_INFO(_visualizer_data_node.get_logger(),
+        "Received Negotiation Trajectory request");
+
+      uint64_t conflict_version = j["param"]["conflict_version"];
+      std::vector<uint64_t> sequence = j["param"]["sequence"];
+
+      auto trajectory_elements =
+        _visualizer_data_node.get_negotiation_trajectories(conflict_version,
+          sequence);
+      const auto now = std::chrono::steady_clock::now();
+
+      RequestParam req;
+      req.start_time = now;
+      req.finish_time = now + 3min;
+
+      response = parse_trajectories("negotiation_trajectory",
+          { { conflict_version } },
+          trajectory_elements, false, req);
+
+      return true;
+    }
+
     else
     {
       return false;
@@ -183,14 +262,16 @@ bool Server::parse_request(server::message_ptr msg, std::string& response)
 }
 
 std::string Server::parse_trajectories(
+  const std::string& response_type,
+  const std::vector<std::vector<uint64_t>>& conflicts,
   const std::vector<Element>& elements,
   const bool trim,
   const RequestParam& request_param)
 {
   std::string response;
   auto j_res = _j_res;
-  j_res["response"] = "trajectory";
-  j_res["conflicts"] = _visualizer_data_node.get_server_conflicts();
+  j_res["response"] = response_type;
+  j_res["conflicts"] = conflicts;
 
   try
   {
@@ -267,9 +348,7 @@ std::string Server::parse_trajectories(
             it->velocity());
         }
       }
-
       j_res["values"].push_back(j_traj);
-
     }
   }
 

rmf_schedule_visualizer/src/Server.hpp

@@ -87,6 +87,8 @@ class Server
   bool parse_request(const server::message_ptr msg, std::string& response);
 
   std::string parse_trajectories(
+    const std::string& response_type,
+    const std::vector<std::vector<uint64_t>>& conflicts,
     const std::vector<Element>& elements,
     const bool trim,
     const RequestParam& request_param);

rmf_schedule_visualizer/src/VisualizerData.cpp

@@ -102,6 +102,9 @@ void VisualizerDataNode::start(Data _data)
       debug_cb(std::move(msg));
     });
 
+  // retrieve/construct mirrors, snapshots and negotiation object
+  _negotiation = rmf_traffic_ros2::schedule::Negotiation(
+    *this, data->mirror.snapshot_handle());
 }
 
 void VisualizerDataNode::debug_cb(std_msgs::msg::String::UniquePtr msg)
@@ -222,4 +225,47 @@ std::vector<std::vector<
   return conflicts;
 }
 
+std::vector<Element> VisualizerDataNode::get_negotiation_trajectories(
+  uint64_t conflict_version, const std::vector<uint64_t>& sequence) const
+{
+  std::vector<Element> trajectory_elements;
+
+  const auto table_view = _negotiation->table_view(conflict_version, sequence);
+  if (!table_view)
+  {
+    RCLCPP_WARN(
+      this->get_logger(), "table_view for conflict %d not found!",
+      conflict_version);
+    return trajectory_elements;
+  }
+
+  rmf_traffic::RouteId route_id = 0;
+  const auto add_route = [&route_id, &table_view, &trajectory_elements]
+      (rmf_traffic::ConstRoutePtr route_ptr,
+      rmf_traffic::schedule::ParticipantId id)
+    {
+      const auto& route = *(route_ptr);
+
+      Element e { id, route_id, route, *table_view->get_description(id) };
+      trajectory_elements.push_back(e);
+      ++route_id;
+    };
+
+  auto itin = table_view->submission();
+  if (itin)
+  {
+    const auto& routes = *itin;
+    for (auto route_ptr : routes)
+      add_route(route_ptr, table_view->participant_id());
+  }
+
+  for (auto proposal : table_view->base_proposals())
+  {
+    for (auto route : proposal.itinerary)
+      add_route(route, proposal.participant);
+  }
+  return trajectory_elements;
+}
+
+
 } // namespace rmf_schedule_visualizer

rmf_schedule_visualizer/src/VisualizerData.hpp

@@ -20,6 +20,7 @@
 #define RMF_SCHEDULE_VISUALIZER__SRC__VISUALIZERDATA_HPP
 
 #include <rmf_traffic_ros2/schedule/MirrorManager.hpp>
+#include <rmf_traffic_ros2/schedule/Negotiation.hpp>
 
 #include <rmf_traffic/Trajectory.hpp>
 #include <rmf_traffic/schedule/Viewer.hpp>
@@ -68,6 +69,9 @@ class VisualizerDataNode : public rclcpp::Node
 
   std::vector<std::vector<uint64_t>> get_server_conflicts() const;
 
+  std::vector<Element> get_negotiation_trajectories(
+    uint64_t conflict_version, const std::vector<uint64_t>& sequence) const;
+
   rmf_traffic::Time now();
 
   std::mutex& get_mutex();
@@ -103,6 +107,8 @@ class VisualizerDataNode : public rclcpp::Node
   std::unordered_map<
     rmf_traffic::schedule::Version,
     std::vector<rmf_traffic::schedule::ParticipantId>> _conflicts;
+
+  rmf_utils::optional<rmf_traffic_ros2::schedule::Negotiation> _negotiation;
 };
 
 } // namespace rmf_schedule_visualizer

rmf_schedule_visualizer/src/main.cpp

@@ -61,9 +61,22 @@ int main(int argc, char* argv[])
   const uint16_t port = port_string.empty() ? 8006 : std::stoul(
     port_string, nullptr, 0);
 
+  std::string retained_history_count_str;
+  uint retained_history_count = 0;
+  if (get_arg(args, "--history", retained_history_count_str,
+    "retained history count", false))
+  {
+    std::stringstream ss;
+    ss << retained_history_count_str;
+    ss >> retained_history_count;
+  }
+
   const auto visualizer_data_node =
     rmf_schedule_visualizer::VisualizerDataNode::make(node_name);
 
+  auto& negotiation = visualizer_data_node->_negotiation;
+  negotiation->set_retained_history_count(retained_history_count);
+
   if (!visualizer_data_node)
   {
     std::cerr << "Failed to initialize the visualizer node" << std::endl;
@@ -74,7 +87,6 @@ int main(int argc, char* argv[])
     visualizer_data_node->get_logger(),
     "VisualizerDataNode /" + node_name + " started...");
 
-
   const auto server_ptr = rmf_schedule_visualizer::Server::make(port,
       *visualizer_data_node);
 

rviz2_plugin/CMakeLists.txt

@@ -22,13 +22,15 @@ set(CMAKE_AUTOMOC ON)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(Eigen3 REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rmf_door_msgs REQUIRED)
 find_package(rmf_lift_msgs REQUIRED)
 find_package(rmf_schedule_visualizer_msgs REQUIRED)
 find_package(rviz_common REQUIRED)
 find_package(rviz_rendering REQUIRED)
 find_package(rviz_default_plugins REQUIRED)
+find_package(rmf_traffic_ros2 REQUIRED)
 find_package(Qt5 REQUIRED COMPONENTS Widgets Test)
 
 add_definitions(-DQT_NO_KEYWORDS)
@@ -50,6 +52,7 @@ target_link_libraries(${PROJECT_NAME}
     ${rviz_rendering_LIBRARIES}
     ${rviz_default_plugins_LIBRARIES}
     ${Qt5Widgets_LIBRARIES}
+    rmf_traffic_ros2::rmf_traffic_ros2
 )
 
 target_include_directories(${PROJECT_NAME}

rviz2_plugin/package.xml

@@ -13,12 +13,14 @@
   <depend>rmf_door_msgs</depend>
   <depend>rmf_lift_msgs</depend>
   <depend>rmf_schedule_visualizer_msgs</depend>
+  <depend>rmf_traffic_ros2</depend>
   <depend>rviz_common</depend>
   <depend>rviz_rendering</depend>
   <depend>rviz_default_plugins</depend>
   <depend>pluginlib</depend>
   <depend>resource_retriever</depend>
 
+  <build_depend>eigen</build_depend>
   <build_depend>qtbase5-dev</build_depend>
   <exec_depend>libqt5-core</exec_depend>
   <exec_depend>libqt5-gui</exec_depend>

visualizer/launch/server_keephistory.xml

@@ -0,0 +1,14 @@
+<?xml version = '1.0' ?>
+
+<launch>
+
+<arg name="use_sim_time" default="true"/>
+<arg name="port" default="8006"/>
+
+<group>
+  <node pkg="rmf_schedule_visualizer" exec="schedule_visualizer" args="-p $(var port) --history 50">
+    <param name="use_sim_time" value="$(var use_sim_time)"/>
+  </node>
+</group>
+
+</launch>