Merge branch '1-implement-required-functionality-of-sumointerface' into 'master'

Resolve "Implement required functionality of SumoInterface"

Closes #1

See merge request ezcar2x/ns3/traci!1

model/traci-commands.h

@@ -240,6 +240,190 @@ private:
 };
 
 
+
+class ChangeLane : public Command
+{
+public:
+
+  ChangeLane(std::string vehicle, Byte lane, double duration)
+    : m_vehicle (vehicle), m_lane (lane), m_duration (duration)
+  {
+  }
+
+private:
+
+  int DoGetId () const override
+  {
+    return libsumo::CMD_SET_VEHICLE_VARIABLE;
+  }
+
+  int DoGetDataLength () const override
+  {
+    return
+	1 + CodingTraits<std::string>::Size (m_vehicle) +
+	1 + CodingTraits<int>::Size (2) + // compound variable
+	2 + // byte type
+	1 + CodingTraits<double>::Size (m_duration); // double type
+  }
+
+  bool DoSerializeData (BufferType& buffer) const override
+  {
+    CodingTraits<UByte>::Append (buffer, libsumo::CMD_CHANGELANE);
+    CodingTraits<std::string>::Append (buffer, m_vehicle);
+    CodingTraits<UByte>::Append (buffer, libsumo::TYPE_COMPOUND);
+    CodingTraits<int>::Append (buffer, 2);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<Byte>::IDENTIFIER);
+    CodingTraits<Byte>::Append (buffer, m_lane);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<double>::IDENTIFIER);
+    CodingTraits<double>::Append (buffer, m_duration);
+    return true;
+  }
+
+  std::string m_vehicle;
+  Byte m_lane;
+  double m_duration;
+};
+
+
+class SetSpeed : public Command
+{
+public:
+
+  SetSpeed(std::string vehicle, double speed)
+    : m_vehicle (vehicle), m_speed (speed)
+  {
+  }
+
+private:
+
+  int DoGetId () const override
+  {
+    return libsumo::CMD_SET_VEHICLE_VARIABLE;
+  }
+
+  int DoGetDataLength () const override
+  {
+    return
+	1 + CodingTraits<std::string>::Size (m_vehicle) +
+	1 + CodingTraits<double>::Size (m_speed);
+  }
+
+  bool DoSerializeData (BufferType& buffer) const override
+  {
+    CodingTraits<UByte>::Append (buffer, libsumo::CMD_SPEED);
+    CodingTraits<std::string>::Append (buffer, m_vehicle);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<double>::IDENTIFIER);
+    CodingTraits<double>::Append (buffer, m_speed);
+    return true;
+  }
+
+  std::string m_vehicle;
+  double m_speed;
+};
+
+
+class SlowDown : public Command
+{
+public:
+
+  SlowDown(std::string vehicle, double speed, double duration)
+    : m_vehicle (vehicle), m_speed (speed), m_duration (duration)
+  {
+  }
+
+private:
+
+  int DoGetId () const override
+  {
+    return libsumo::CMD_SET_VEHICLE_VARIABLE;
+  }
+
+  int DoGetDataLength () const override
+  {
+    return
+	1 + CodingTraits<std::string>::Size (m_vehicle) +
+	1 + CodingTraits<int>::Size (2) + // compound variable
+	1 + CodingTraits<double>::Size (m_speed) + // double type
+	1 + CodingTraits<double>::Size (m_duration); // double type
+  }
+
+  bool DoSerializeData (BufferType& buffer) const override
+  {
+    CodingTraits<UByte>::Append (buffer, libsumo::CMD_SLOWDOWN);
+    CodingTraits<std::string>::Append (buffer, m_vehicle);
+    CodingTraits<UByte>::Append (buffer, libsumo::TYPE_COMPOUND);
+    CodingTraits<int>::Append (buffer, 2);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<double>::IDENTIFIER);
+    CodingTraits<double>::Append (buffer, m_speed);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<double>::IDENTIFIER);
+    CodingTraits<double>::Append (buffer, m_duration);
+    return true;
+  }
+
+  std::string m_vehicle;
+  double m_speed;
+  double m_duration;
+};
+
+
+class MoveToXY : public Command
+{
+public:
+
+  MoveToXY(std::string vehicle, std::string edge, int lane, double x, double y, double angle)
+    : m_vehicle (vehicle), m_edge (edge), m_lane (lane), m_x (x), m_y (y), m_angle (angle)
+  {
+  }
+
+private:
+
+  int DoGetId () const override
+  {
+    return libsumo::CMD_SET_VEHICLE_VARIABLE;
+  }
+
+  int DoGetDataLength () const override
+  {
+    return
+	1 + CodingTraits<std::string>::Size (m_vehicle) +
+	1 + CodingTraits<int>::Size (5) + // compound variable
+	1 + CodingTraits<std::string>::Size (m_edge) + // string type
+	1 + CodingTraits<int>::Size (m_lane) + // int type
+        1 + CodingTraits<double>::Size (m_x) + // double type
+        1 + CodingTraits<double>::Size (m_y) + // double type
+	1 + CodingTraits<double>::Size (m_angle); // double type
+  }
+
+  bool DoSerializeData (BufferType& buffer) const override
+  {
+    CodingTraits<UByte>::Append (buffer, libsumo::MOVE_TO_XY);
+    CodingTraits<std::string>::Append (buffer, m_vehicle);
+    CodingTraits<UByte>::Append (buffer, libsumo::TYPE_COMPOUND);
+    CodingTraits<int>::Append (buffer, 5);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<std::string>::IDENTIFIER);
+    CodingTraits<std::string>::Append (buffer, m_edge);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<int>::IDENTIFIER);
+    CodingTraits<int>::Append (buffer, m_lane);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<double>::IDENTIFIER);
+    CodingTraits<double>::Append (buffer, m_x);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<double>::IDENTIFIER);
+    CodingTraits<double>::Append (buffer, m_y);
+    CodingTraits<UByte>::Append (buffer, CodingTraits<double>::IDENTIFIER);
+    CodingTraits<double>::Append (buffer, m_angle);
+    return true;
+  }
+
+  std::string m_vehicle;
+  std::string m_edge;
+  int m_lane;
+  double m_x;
+  double m_y;
+  double m_angle;
+
+};
+
+
+
 }  // namespace command
 }  // namespace traci
 }  // namespace ns3

model/traci-constants.h

@@ -79,6 +79,9 @@ TRACI_CONST int CMD_OPENGAP = 0x16;
 // command: change target
 TRACI_CONST int CMD_CHANGETARGET = 0x31;
 
+// command: set speed
+TRACI_CONST int CMD_SPEED = 0x40;
+
 // command: close sumo
 TRACI_CONST int CMD_CLOSE = 0x7F;
 
@@ -86,6 +89,7 @@ TRACI_CONST int CMD_CLOSE = 0x7F;
 TRACI_CONST int CMD_ADD_SUBSCRIPTION_FILTER = 0x7e;
 
 
+
 // command: subscribe induction loop (e1) context
 TRACI_CONST int CMD_SUBSCRIBE_INDUCTIONLOOP_CONTEXT = 0x80;
 // response: subscribe induction loop (e1) context

model/traci-results.h

@@ -137,6 +137,46 @@ private:
 };
 
 
+/*!
+ * @brief Status response data structure.
+ */
+class Status {
+public:
+  Status() : m_command(0), m_result(0), m_description("") {}
+
+  void Deserialize (BufferType::const_iterator it, int bytesRemaining)
+    {
+      if (bytesRemaining < (1 + 1 + 1 + 4))
+        {
+          /*
+           * We expect at least:
+           * length, command id, api version, identifier string length
+           */
+          throw SerializationError("expected length exceeds buffer size");
+        }
+
+      int length = CodingTraits<UByte>::Read (it);
+
+      if (bytesRemaining < length)
+        {
+          throw SerializationError("length exceeds buffer size");
+        }
+
+      m_command = CodingTraits<UByte>::Read (it); // reply command id
+
+      m_result = CodingTraits<UByte>::Read (it);
+      m_description = CodingTraits<std::string>::Read (it);
+
+    }
+
+  UByte GetResult () const { return m_result; }
+  std::string GetDescription () const { return m_description; }
+
+private:
+  UByte m_command;
+  UByte m_result;
+  std::string m_description;
+};
 
 
 

model/traci-vehicle.cc

@@ -17,10 +17,14 @@
  *
  * Author: Karsten Roscher <karsten.roscher@esk.fraunhofer.de>
  */
-#include "traci-vehicle.h"
 
+#include "traci-vehicle.h"
 #include "traci-geometry.h"
 
+#include "ns3/log.h"
+
+NS_LOG_COMPONENT_DEFINE("TraCiVehicle");
+
 namespace ns3 {
 namespace traci {
 
@@ -28,8 +32,8 @@ Vehicle::Vehicle (Ptr<TraCi> traci, std::string id)
   : m_traci (traci), m_id (id),
     m_position (m_traci, id),
     m_length (m_traci, id), m_width (m_traci, id), m_height (m_traci, id),
-    m_speed (m_traci, id), m_maxSpeed (m_traci, id), m_heading (m_traci, id),
-    m_color (m_traci, id)
+    m_speed (m_traci, id), m_speed_lateral (m_traci, id), m_maxSpeed (m_traci, id),
+    m_heading (m_traci, id), m_acceleration (m_traci, id), m_color (m_traci, id)
 {
 }
 
@@ -112,6 +116,12 @@ Vehicle::GetMaxSpeed (bool forceReload) const
   return m_maxSpeed.GetValue (forceReload);
 }
 
+double
+Vehicle::GetSpeedLateral (bool forceReload) const
+{
+  return m_speed_lateral.GetValue (forceReload);
+}
+
 Heading
 Vehicle::GetHeading (bool forceReload) const
 {
@@ -127,6 +137,12 @@ Vehicle::GetVelocity (bool forceReload) const
   return VelocityFromSpeedHeading (speed, heading);
 }
 
+double
+Vehicle::GetAcceleration (bool forceReload) const
+{
+  return m_acceleration.GetValue (forceReload);
+}
+
 Color
 Vehicle::GetColor (bool forceReload) const
 {
@@ -139,5 +155,53 @@ Vehicle::SetColor (const Color& color)
   SetVariable (libsumo::VAR_COLOR, color);
 }
 
+void
+Vehicle::changeLane (int laneID,
+                         double duration)
+{
+    NS_LOG_INFO("TraCiVehicle: Sending ChangeLane to vehicle "+m_id+" : lane = "+std::to_string(laneID)+" duration = "+std::to_string(duration));
+    traci::Byte lane = (traci::Byte)laneID;
+    traci::command::ChangeLane my_command(m_id, lane, duration);
+    traci::result::NoResult my_result;
+    m_traci->ExecuteCommand (my_command, my_result);
+}
+
+void
+Vehicle::setSpeed (double speed)
+{
+    NS_LOG_INFO("TraCiVehicle: Sending SetSpeed to vehicle "+m_id+" : speed = "+std::to_string(speed));
+    traci::command::SetSpeed my_command(m_id, speed);
+    traci::result::NoResult my_result;
+    m_traci->ExecuteCommand (my_command, my_result);
+}
+
+void
+Vehicle::slowDown (double speed, double duration)
+{
+    NS_LOG_INFO("TraCiVehicle: Sending SlowDown to vehicle "+m_id+" : speed = "+std::to_string(speed)+" duration = "+std::to_string(duration));
+    traci::command::SlowDown my_command(m_id, speed, duration);
+    traci::result::NoResult my_result;
+    m_traci->ExecuteCommand (my_command, my_result);
+}
+
+void
+Vehicle::moveToXY (std::string edgeID, int laneID, double x, double y, double angle)
+{
+    if (angle < -360.0)
+    {
+        angle = libsumo::INVALID_DOUBLE_VALUE;
+        NS_LOG_INFO("TraCiVehicle: Sending MoveToXY to vehicle "+m_id+" : edge = "+edgeID+" lane = "+std::to_string(laneID)+" x = "+std::to_string(x)+" y = "+std::to_string(y));
+    }
+    else
+    {
+        if (angle < 0) angle += 360.0;
+        NS_LOG_INFO("TraCiVehicle: Sending MoveToXY to vehicle "+m_id+" : edge = "+edgeID+" lane = "+std::to_string(laneID)+" x = "+std::to_string(x)+" y = "+std::to_string(y)+" angle = "+std::to_string(angle));
+    }
+    traci::command::MoveToXY my_command(m_id, edgeID, laneID, x, y, angle);
+    traci::result::NoResult my_result;
+    m_traci->ExecuteCommand (my_command, my_result);
+}
+
+
 }
 }

model/traci-vehicle.h

@@ -129,6 +129,10 @@ public:
   double
   GetMaxSpeed (bool forceReload = false) const;
 
+  //! Get lateral speed in m/s
+  double
+  GetSpeedLateral (bool forceReload = false) const;
+
   //! Get current heading
   Heading
   GetHeading (bool forceReload = false) const;
@@ -137,6 +141,10 @@ public:
   Vector
   GetVelocity (bool forceReload = false) const;
 
+  //! Get current acceleration
+  double
+  GetAcceleration (bool forceReload = false) const;
+
   Color
   GetColor (bool forceReload = false) const;
 
@@ -183,7 +191,50 @@ public:
   template<typename VariableType>
   void
   SetVariable (int variable, VariableType value);
-
+  
+  
+  /*!
+   * @brief change lane of the specified vehicle over given duration.
+   *
+   * Parameters:
+   * - ID of the lange to change to
+   * - duration for the change
+   */
+  void
+  changeLane (int laneID, double duration);
+  
+  /*!
+   * @brief set speed of the specified vehicle.
+   *
+   * Parameters:
+   * - new speed to set
+   */
+  void
+  setSpeed (double speed);
+  
+  /*!
+   * @brief gradually change the speed over given duration.
+   *
+   * Parameters:
+   * - new speed to set
+   * - duration for the change
+   */
+  void
+  slowDown (double speed, double duration);
+  
+  /*!
+   * @brief set new position for vehicle.
+   *
+   * Parameters:
+   * - ID of the road
+   * - ID of the lange to change to
+   * - x coordinate
+   * - y coordinate
+   * - angle (if default value, then use the angle of the edge)
+   */
+  void
+  moveToXY (std::string edgeID, int laneID, double x, double y, double angle=-1000000.0);
+  
 private:
   Ptr<TraCi> m_traci;	// link to the traci instance
   std::string m_id;	// id of the object
@@ -197,8 +248,10 @@ private:
   CachedVariable<double, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_WIDTH, true> m_width;
   CachedVariable<double, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_HEIGHT, true> m_height;
   CachedVariable<double, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_SPEED> m_speed;
+  CachedVariable<double, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_SPEED_LAT> m_speed_lateral;
   CachedVariable<double, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_MAXSPEED> m_maxSpeed;
   CachedVariable<double, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_ANGLE> m_heading;
+  CachedVariable<double, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_ACCELERATION> m_acceleration;
   CachedVariable<Color, libsumo::CMD_GET_VEHICLE_VARIABLE, libsumo::VAR_COLOR> m_color;
 };