SimulationAdapterNode.cpp

Go to the documentation of this file.

#include <adapter/SimulationAdapterNode.hpp>
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include <galaxis/controllerSettings.hpp>
 
#include <unistd.h>
#include <thread>
#include <cstdlib>
using namespace std;
 
using namespace galaxis::ros;
using namespace galaxis::utils;
 
SimulationAdapterNode::SimulationAdapterNode() :
    Node("SimulationAdapter") {
 
  log("SimulationAdapterNode initializing:");
 
  m_carPose              = std::make_shared<CarPose>();
  m_carPose->position    = Eigen::Vector2f(0.4, -0.2); // TODO: set initial position (initial_pose: [0.4, -0.2])
  m_carPose->orientation = 0;
  m_remoteControl        = false;
 
  m_yawRate        = 0.f;
  m_speedPerSecond = 0.f;
 
  // From node_ros_pubsub_wrapper:
  // enum galaxis::ros::QoS_Profile_t {
  //     QOS_DEFAULT     = 0,
  //     QOS_BEST_EFFORT = 1,
  //     QOS_RELIABLE    = 2
  //   };
 
  // Publisher for KITCAR-SIMULATION (/simulation/gazebo/model/NAME_OF_MODEL/set_pose with NAME_OF_MODEL:=dr_drift)
  rclcpp::QoS qos_params          = get_qos_profile(QOS_BEST_EFFORT);
  m_simulationCarRealStatePubSPtr = this->create_publisher<gazebo_simulation::msg::SetModelPose>("/simulation/gazebo/model/dr_drift/set_pose", qos_params);
  log("Added gazebo_simulation::msg::SetModelPose Publisher.");
 
  m_vehicleControlSubSPtr = this->create_subscription<SimulationVehicleControlType>("galaxis/simulation/remotecontrol", qos_params, std::bind(&SimulationAdapterNode::vehicleControlCallback, this, std::placeholders::_1));
 
  log("Start simulationUpdateLoop and updatePose in second thread and detach");
  std::thread(&SimulationAdapterNode::simulationUpdateLoop, this).detach();
  std::thread(&SimulationAdapterNode::updatePose, this).detach();
}
 
SimulationAdapterNode::~SimulationAdapterNode() {
}
 
// Updates the KITCAR-GAZEBO-SIMULATION
void SimulationAdapterNode::simulationUpdateLoop() {
  while(true) {
    auto time = rclcpp::Clock().now();
    log("[", static_cast<int>(time.seconds()) % 160742000000000, "] Send random message.");
 
    auto msgCarRealStateUPtr = std::make_unique<gazebo_simulation::msg::SetModelPose>();
 
    m_carPoseMtx.lock();
 
    // position
    msgCarRealStateUPtr->keys.push_back(gazebo_simulation::msg::SetModelPose::POSITION_X);
    msgCarRealStateUPtr->keys.push_back(gazebo_simulation::msg::SetModelPose::POSITION_Y);
 
    msgCarRealStateUPtr->values.push_back(m_carPose->position[0]);
    msgCarRealStateUPtr->values.push_back(m_carPose->position[1]);
 
    // orientation
    Eigen::AngleAxisf rollAngle(0, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf yawAngle(m_carPose->orientation, Eigen::Vector3f::UnitZ());
    Eigen::AngleAxisf pitchAngle(0, Eigen::Vector3f::UnitX());
 
    Eigen::Quaternionf quaternion = rollAngle * yawAngle * pitchAngle;
 
    msgCarRealStateUPtr->keys.push_back(gazebo_simulation::msg::SetModelPose::ORIENTATION_W);
    msgCarRealStateUPtr->keys.push_back(gazebo_simulation::msg::SetModelPose::ORIENTATION_X);
    msgCarRealStateUPtr->keys.push_back(gazebo_simulation::msg::SetModelPose::ORIENTATION_Y);
    msgCarRealStateUPtr->keys.push_back(gazebo_simulation::msg::SetModelPose::ORIENTATION_Z);
 
    msgCarRealStateUPtr->values.push_back(quaternion.w());
    msgCarRealStateUPtr->values.push_back(quaternion.x());
    msgCarRealStateUPtr->values.push_back(quaternion.y());
    msgCarRealStateUPtr->values.push_back(quaternion.z());
 
    m_carPoseMtx.unlock();
 
    m_simulationCarRealStatePubSPtr->publish(std::move(msgCarRealStateUPtr));
 
    usleep(8000);
  }
 
  log("Killed");
}
 
void SimulationAdapterNode::vehicleControlCallback(SimulationVehicleControlType::SharedPtr msg) {
  if(msg->remote_control == 1) {
    log("[REMOTECONTROL] Enabled. Only drive with remote control.");
    m_remoteControl = true;
    m_speedPerSecond = msg->car_speed;
    m_yawRate        = msg->turnangle_front;
  }
  else if(msg->remote_control == 0) {
    m_remoteControl = false;
    m_speedPerSecond = msg->car_speed;
    m_yawRate        = msg->turnangle_front;
  }
  
  if (!m_remoteControl) {
    m_speedPerSecond = msg->car_speed;
    m_yawRate        = msg->turnangle_front;
  }
 
  if(msg->reset == true) {
    m_carPoseMtx.lock();
 
    m_speedPerSecond       = 0.f;
    m_yawRate              = 0.f;
    m_carPose->position[0] = 0.034f;
    m_carPose->position[1] = 0.f;
    m_carPose->orientation = 0.f;
 
    m_carPoseMtx.unlock();
  }
 
  log("Vehicle Control Message received. Speed: ", m_speedPerSecond, " Yaw: ", m_yawRate);
}
 
void SimulationAdapterNode::updatePose() {
  rclcpp::Time lastUpdateTime = rclcpp::Clock().now();
 
  while(true) {
    m_carPoseMtx.lock();
 
    auto currentTime        = rclcpp::Clock().now();
    float distanceTravelled = m_speedPerSecond * (currentTime - lastUpdateTime).seconds();
    log("UpdatePose - DistanceTravelled: ", distanceTravelled);
    lastUpdateTime = currentTime;
 
    std::shared_ptr<CarPose> m_carPoseTemp = get_new_pose(m_carPose, galaxis::controllerSettings::carWheelbaseSimulationDrDrift, distanceTravelled, m_yawRate);
    m_carPose                              = m_carPoseTemp;
 
    // ####################### Convert Position to 
 
    log("X: ", m_carPose->position[0], "Y: ", m_carPose->position[1]);
 
    m_carPoseMtx.unlock();
    usleep(4000);
  }
}