traci-vehicle.cc

/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2017 Fraunhofer ESK
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Author: Karsten Roscher <karsten.roscher@esk.fraunhofer.de>
 */

#include "traci-vehicle.h"
#include "traci-geometry.h"

#include "ns3/log.h"

NS_LOG_COMPONENT_DEFINE("TraCiVehicle");

namespace ns3 {
namespace traci {

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_speedLateral (m_traci, id), m_maxSpeed (m_traci, id),
    m_maxSpeedLateral (m_traci, id), m_heading (m_traci, id), m_acceleration (m_traci, id),
    m_maxAcceleration (m_traci, id), m_maxDeceleration (m_traci, id),
    m_laneIdx (m_traci, id), m_roadId (m_traci, id), m_routeId (m_traci, id), m_route (m_traci, id), m_color (m_traci, id)
{
}

std::string
Vehicle::GetId () const
{
  return m_id;
}

Ptr<TraCi>
Vehicle::GetTraCi () const
{
  return  m_traci;
}

void
Vehicle::SetNode (Ptr<Node> node)
{
  m_node = node;
}

Ptr<Node>
Vehicle::GetNode () const
{
  return m_node;
}

Vector
Vehicle::GetPosition (bool forceReload) const
{
  // Note: This is ok for now since Position3D is just a typedef for Vector.
  //       If it changes some day, the compiler should complain.
  return m_position.GetValue (forceReload);
}

Vector2D
Vehicle::GetPosition2d (bool forceReload) const
{
  // we reuse the 3D position here
  Vector pos = GetPosition (forceReload);
  return Vector2D (pos.x, pos.y);
}

Vector
Vehicle::GetCenterPosition (bool forceReload) const
{
  return ShiftPosition (
      GetPosition (forceReload),
      GetHeading (forceReload),
      -GetLength (forceReload) / 2.0);
}

double
Vehicle::GetLength (bool forceReload) const
{
  return m_length.GetValue (forceReload);
}

double
Vehicle::GetWidth (bool forceReload) const
{
  return m_width.GetValue (forceReload);
}

double
Vehicle::GetHeight (bool forceReload) const
{
  return m_height.GetValue (forceReload);
}

double
Vehicle::GetSpeed (bool forceReload) const
{
  return m_speed.GetValue (forceReload);
}

double
Vehicle::GetMaxSpeed (bool forceReload) const
{
  return m_maxSpeed.GetValue (forceReload);
}

double
Vehicle::GetSpeedLateral (bool forceReload) const
{
  return m_speedLateral.GetValue (forceReload);
}

double
Vehicle::GetMaxSpeedLateral (bool forceReload) const
{
  return m_maxSpeedLateral.GetValue (forceReload);
}

Heading
Vehicle::GetHeading (bool forceReload) const
{
  return Heading (m_heading.GetValue (forceReload));
}

Vector
Vehicle::GetVelocity (bool forceReload) const
{
  double speed = GetSpeed (forceReload);
  Heading heading = GetHeading (forceReload);

  return VelocityFromSpeedHeading (speed, heading);
}

double
Vehicle::GetAcceleration (bool forceReload) const
{
  return m_acceleration.GetValue (forceReload);
}

double
Vehicle::GetMaxAcceleration (bool forceReload) const
{
  return m_maxAcceleration.GetValue (forceReload);
}

double
Vehicle::GetMaxDeceleration (bool forceReload) const
{
  return m_maxDeceleration.GetValue (forceReload);
}

int
Vehicle::GetLaneIndex (bool forceReload) const
{
  return m_laneIdx.GetValue (forceReload);
}

std::string
Vehicle::GetRoadID (bool forceReload) const
{
  return m_roadId.GetValue (forceReload);
}

std::string
Vehicle::GetRouteID (bool forceReload) const
{
  return m_routeId.GetValue (forceReload);
}

std::vector<std::string>
Vehicle::GetRoute (bool forceReload) const
{
  return m_route.GetValue (forceReload);
}

Color
Vehicle::GetColor (bool forceReload) const
{
  return m_color.GetValue (forceReload);
}

void
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::setMaxSpeedLateral (double max_speed)
{
    NS_LOG_INFO("TraCiVehicle: Sending SetMaxSpeedLateral to vehicle "+m_id+" : max_speed = "+std::to_string(max_speed));
    traci::command::SetMaxSpeedLateral my_command(m_id, max_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);
}

void
Vehicle::setRoute (std::vector<std::string> edgeIdList)
{
    NS_LOG_INFO("TraCiVehicle: Sending route to vehicle "+m_id+" : route = ");
    for (auto const& edgeId : edgeIdList) {
        NS_LOG_INFO("EdgeId: "+ edgeId+ " ");
    }
    traci::command::SetRoute my_command(m_id, edgeIdList);
    traci::result::NoResult my_result;
    m_traci->ExecuteCommand (my_command, my_result);
}

void
Vehicle::setStop (std::string edgeID, double endpos, int lane, double stopduration, int stopflag, double startpos, double stopuntil)
{
    NS_LOG_INFO("TraCiVehicle: Sending stop to vehicle "+m_id+" : stop at = "+edgeID+", for = "+std::to_string(stopduration)+"sec or until = "+std::to_string(stopuntil));

    traci::command::SetStop my_command(m_id, edgeID, endpos, (traci::Byte)lane, stopduration, (traci::Byte)stopflag, startpos, stopuntil);
    traci::result::NoResult my_result;
    m_traci->ExecuteCommand (my_command, my_result);

}

}
}