Galaxis Codebase: SimulationAdapterNode Class Reference

#include <SimulationAdapterNode.hpp>

Inheritance diagram for SimulationAdapterNode:

Collaboration diagram for SimulationAdapterNode:

Public Member Functions
	SimulationAdapterNode ()

	~SimulationAdapterNode ()

Private Member Functions
void	simulationUpdateLoop ()

void	updatePose ()

void	vehicleControlCallback (SimulationVehicleControlType::SharedPtr msg)

Private Attributes
std::shared_ptr< CarPose >	m_carPose

std::atomic< float >	m_yawRate

std::atomic< float >	m_speedPerSecond

std::atomic< bool >	m_remoteControl

rclcpp::Publisher< gazebo_simulation::msg::SetModelPose >::SharedPtr	m_simulationCarRealStatePubSPtr

rclcpp::Subscription< ControlMotionType >::SharedPtr	m_controlValuesSubSPtr

rclcpp::Subscription< SimulationVehicleControlType >::SharedPtr	m_vehicleControlSubSPtr

std::mutex	m_carPoseMtx

Detailed Description

Definition at line 28 of file SimulationAdapterNode.hpp.

Constructor & Destructor Documentation

◆ SimulationAdapterNode()

SimulationAdapterNode::SimulationAdapterNode ( )

Definition at line 13 of file SimulationAdapterNode.cpp.

                                              :
     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();
 }

References galaxis::utils::log(), m_carPose, m_remoteControl, m_simulationCarRealStatePubSPtr, m_speedPerSecond, m_vehicleControlSubSPtr, m_yawRate, simulationUpdateLoop(), updatePose(), and vehicleControlCallback().

Here is the call graph for this function:

◆ ~SimulationAdapterNode()

SimulationAdapterNode::~SimulationAdapterNode ( )

Definition at line 45 of file SimulationAdapterNode.cpp.

45 {

46 }

Member Function Documentation

◆ simulationUpdateLoop()

void SimulationAdapterNode::simulationUpdateLoop ( )

private

Definition at line 49 of file SimulationAdapterNode.cpp.

                                                  {
   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");
 }

References galaxis::utils::log(), m_carPose, m_carPoseMtx, and m_simulationCarRealStatePubSPtr.

Referenced by SimulationAdapterNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updatePose()

void SimulationAdapterNode::updatePose ( )

private

Definition at line 125 of file SimulationAdapterNode.cpp.

                                        {
   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);
   }
 }

References galaxis::controllerSettings::carWheelbaseSimulationDrDrift, galaxis::control::get_new_pose(), galaxis::utils::log(), m_carPose, m_carPoseMtx, m_speedPerSecond, and m_yawRate.

Referenced by SimulationAdapterNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ vehicleControlCallback()

void SimulationAdapterNode::vehicleControlCallback ( SimulationVehicleControlType::SharedPtr msg )

private

Definition at line 92 of file SimulationAdapterNode.cpp.

                                                                                             {
   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);
 }