Bye arm state

modules/application/include/dls2/application/periodic_app.hpp

@@ -4,6 +4,7 @@
 #include "dls2/application/app.hpp"
 #include <dls2/util/time/time.hpp>
 #include "dls2/util/process_resource_monitor.hpp"
+#include "dls2/util/numerical_moving_window.hpp"
 
 #include <boost/process.hpp>
 #include <yaml-cpp/yaml.h>
@@ -124,6 +125,7 @@ namespace dls
 		std::chrono::time_point<std::chrono::steady_clock> loop_time_prec;
 
 		std::unique_ptr<ProcessResourceMonitor> process_resource_monitor_;
+		std::unique_ptr<NumericalMovingWindow<double>> frequency_moving_window_;
 
 		double current_frequency_;
 		std::mutex frequency_mutex_;

modules/application/src/periodic_app.cpp

@@ -1,5 +1,7 @@
 #include "dls2/application/periodic_app.hpp"
 
+#include <cmath>
+
 using namespace dls;
 
 PeriodicApp::PeriodicApp(const std::string &ID) 
@@ -68,6 +70,8 @@ PeriodicApp::PeriodicApp(const std::string &ID)
 
 	process_resource_monitor_ = std::make_unique<ProcessResourceMonitor>(
 						this->pid, this->safety_layer_config_->process_monitor_window_size);
+	frequency_moving_window_ = std::make_unique<NumericalMovingWindow<double>>(
+						this->safety_layer_config_->process_frequency_window_size);
 }
 
 std::string PeriodicApp::getSchedulerPath(const std::string &ID)
@@ -251,7 +255,13 @@ void PeriodicApp::checkRT()
 {
 	{
 		std::lock_guard<std::mutex> lock(this->frequency_mutex_);
-		realtime_curr = Time::checkFrequency(this->safety_layer_config_->realtime_tolerance_factor, getDesiredFrequency(), loop_time_prec, current_frequency_);
+		double instantaneous_frequency = 0.0;
+		Time::checkFrequency(this->safety_layer_config_->realtime_tolerance_factor,
+			getDesiredFrequency(), loop_time_prec, instantaneous_frequency);
+		frequency_moving_window_->push(instantaneous_frequency);
+		current_frequency_ = frequency_moving_window_->mean();
+		realtime_curr = std::abs(getDesiredFrequency() - current_frequency_) <
+			this->safety_layer_config_->realtime_tolerance_factor * getDesiredFrequency();
 	}
 
 	// notify if the process is not running in real time

modules/controller/include/dls2/controller/control_modes.hpp

@@ -0,0 +1,10 @@
+#pragma once
+
+#include <stdint.h>
+
+enum class ControlMode : uint8_t {
+        TORQUE_MODE = 0,
+		IMPEDANCE_MODE,
+		POSITION_MODE,
+		VELOCITY_MODE
+};

modules/controller/include/dls2/controller/controller.hpp

@@ -6,6 +6,7 @@
 // =============================================================================
 #include "dls2/application/periodic_app.hpp"
 #include "dls2/controller/controller_data.hpp"
+#include "dls2/controller/control_modes.hpp"
 
 #include "dls2/util/messaging/dds_participant.hpp"
 

modules/controller/include/dls2/controller/controller_data.hpp

@@ -5,6 +5,7 @@
 #include "dls2/signal/reader.hpp"
 #include "dls2/math/ramp.hpp"
 #include "robotlib/robot_base.hpp"
+#include "dls2/controller/control_modes.hpp"
 
 #include <memory>
 

modules/core_framework/include/dls2/core_framework/control_layer.hpp

@@ -79,7 +79,9 @@ class ControlLayer : public Layer
 	/// @return true if the generator unloads correctly.
 	bool unloadMotionGenerator(const std::string&);
 
-	/// Returns the last published desired torques
+	/// Returns the last published joint reference
+	std::vector<double> getPublishedDesiredPosition();
+	std::vector<double> getPublishedDesiredVelocity();
 	std::vector<double> getPublishedDesiredTorques();
 
 private:
@@ -118,9 +120,9 @@ class ControlLayer : public Layer
 	std::shared_ptr<robotlib::RobotBase> pRobot;
 
 	/// @brief Output signals
-	Writer<dls2_interface::msg::DesiredTorques> writer_control_signal;
+	Writer<dls2_interface::msg::ControlSignal> writer_control_signal;
 
-	dls2_interface::msg::DesiredTorques torques;
+	dls2_interface::msg::ControlSignal control_signal_msg;
 
 	bool unloadController(std::shared_ptr<ControllerData> pData);
 

modules/core_framework/src/control_layer.cpp.in

@@ -57,11 +57,21 @@ ControlLayer::ControlLayer(std::string ID, std::string robot_name)
 	, pRobot(robotlib::RobotFactory::openRobot(robot_name))
 	, writer_control_signal(
 		ddsSignalLink, 
-		dls::topics::desired_torques)
-	, torques()
+		dls::topics::control_signal)
+	, control_signal_msg()
 {
-	writer_control_signal.msg.desired_torques().resize(pRobot->getNJOINTS());
-	torques.desired_torques().resize(pRobot->getNJOINTS());
+	writer_control_signal.msg.joints_position().resize(pRobot->getNJOINTS());
+	writer_control_signal.msg.joints_velocity().resize(pRobot->getNJOINTS());
+	writer_control_signal.msg.joints_torques().resize(pRobot->getNJOINTS());
+	writer_control_signal.msg.kp().resize(pRobot->getNJOINTS());
+	writer_control_signal.msg.kd().resize(pRobot->getNJOINTS());
+
+	control_signal_msg.joints_position().resize(pRobot->getNJOINTS());
+	control_signal_msg.joints_velocity().resize(pRobot->getNJOINTS());
+	control_signal_msg.joints_torques().resize(pRobot->getNJOINTS());
+	control_signal_msg.kp().resize(pRobot->getNJOINTS());
+	control_signal_msg.kd().resize(pRobot->getNJOINTS());
+
 	command_manager.addCommand<std::string>
 	(
 		"loadGenerator",
@@ -238,18 +248,77 @@ void *ControlLayer::controlSignalGather(void *data)
         auto epoch = std::chrono::time_point_cast<std::chrono::milliseconds>(now).time_since_epoch();
 
 		// Read the control signals
-		std::fill(layer->torques.desired_torques().begin(), layer->torques.desired_torques().end(), 0);
-
-		for(auto pair : layer->controllers)
+		std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0);
+		layer->control_signal_msg.control_mode() = static_cast<uint8_t>(ControlMode::TORQUE_MODE);
+		bool active_position_controller = false; 
+		bool active_velocity_controller = false;
+		bool active_impedence_controller = false;
+		bool control_mode_set_once = false;
+
+		for(auto [_, controller_data] : layer->controllers)
 		{
-			pair.second->reader_control_signal.read();
-			for(long unsigned int i=0; i<pair.second->reader_control_signal.msg.torques().size();i++){
-				layer->torques.desired_torques()[i] += pair.second->premultiplier * pair.second->reader_control_signal.msg.torques()[i];
+			controller_data->reader_control_signal.read();
+			const auto& controller_msg = controller_data->reader_control_signal.msg;
+			const auto controller_mode = static_cast<ControlMode>(controller_msg.control_mode());
+
+			if(!control_mode_set_once){
+				layer->control_signal_msg.control_mode() = controller_msg.control_mode();
+				control_mode_set_once = true;
+			}
+
+			const auto aggregate_mode = static_cast<ControlMode>(layer->control_signal_msg.control_mode());
+			bool torque_controller_in_torque_mode = controller_mode == ControlMode::TORQUE_MODE && aggregate_mode == ControlMode::TORQUE_MODE;
+			bool torque_controller_in_impedence_mode = aggregate_mode == ControlMode::IMPEDANCE_MODE && controller_mode == ControlMode::TORQUE_MODE;
+			bool first_impedence_controller = controller_mode == ControlMode::IMPEDANCE_MODE && !active_impedence_controller;
+			bool first_position_controller = controller_mode == ControlMode::POSITION_MODE && !active_position_controller;
+			bool first_velocity_controller = controller_mode == ControlMode::VELOCITY_MODE && !active_velocity_controller;
+
+
+			if(first_position_controller){
+				layer->control_signal_msg.control_mode() = static_cast<uint8_t>(ControlMode::POSITION_MODE);
+				layer->control_signal_msg.joints_position() = controller_msg.joints_position();
+
+				// Reset all other fields to zero
+				std::fill(layer->control_signal_msg.joints_velocity().begin(), layer->control_signal_msg.joints_velocity().end(), 0);
+				std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0);
+				std::fill(layer->control_signal_msg.kp().begin(), layer->control_signal_msg.kp().end(), 0.0);
+				std::fill(layer->control_signal_msg.kd().begin(), layer->control_signal_msg.kd().end(), 0.0);
+				break;
+			}
+
+			if(first_velocity_controller){
+				layer->control_signal_msg.control_mode() = static_cast<uint8_t>(ControlMode::VELOCITY_MODE);
+				layer->control_signal_msg.joints_velocity() = controller_msg.joints_velocity();
+
+				// Reset all other fields to zero
+				std::fill(layer->control_signal_msg.joints_position().begin(), layer->control_signal_msg.joints_position().end(), 0);
+				std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0);
+				std::fill(layer->control_signal_msg.kp().begin(), layer->control_signal_msg.kp().end(), 0.0);
+				std::fill(layer->control_signal_msg.kd().begin(), layer->control_signal_msg.kd().end(), 0.0);
+				break;
+			}
+
+
+			if(torque_controller_in_torque_mode || torque_controller_in_impedence_mode || first_impedence_controller){
+				if(first_impedence_controller){
+					active_impedence_controller = true;
+					layer->control_signal_msg.control_mode() = static_cast<uint8_t>(ControlMode::IMPEDANCE_MODE);
+					layer->control_signal_msg.joints_position() = controller_msg.joints_position();
+					layer->control_signal_msg.joints_velocity() = controller_msg.joints_velocity();
+					layer->control_signal_msg.kp() = controller_msg.kp();
+					layer->control_signal_msg.kd() = controller_msg.kd();
+				}
+
+				for(size_t i = 0; i < controller_msg.joints_torques().size();i++){
+					// Torque contributions summed up
+					layer->control_signal_msg.joints_torques()[i] += controller_data->premultiplier * controller_msg.joints_torques()[i];
+				}
 			}
 		}
 
-    	// Send the desired torques to HAL
-		layer->writer_control_signal.msg.desired_torques() = layer->saturateTorques(layer->torques.desired_torques());
+    	// Fill in the control signal to be sent to HAL
+		layer->control_signal_msg.joints_torques() = layer->saturateTorques(layer->control_signal_msg.joints_torques());
+		layer->writer_control_signal.msg = layer->control_signal_msg;
 		layer->writer_control_signal.msg.timestamp() = std::chrono::duration_cast<std::chrono::microseconds>(epoch).count();
 		layer->writer_control_signal.publish();
 
@@ -263,7 +332,6 @@ void *ControlLayer::controlSignalGather(void *data)
 	    {
             std::stringstream ss;
 		    ss << "Control Layer is probably breaking real-time. Has period of 1 mseconds but spent: " << mean << " useconds ";
-  		    // ss << "Control layer published torques " << desired_torques.transpose(); // << std::endl;
             layer->app_logger.warning(ss.str(), EventID::WRONG_PROCESS_FREQUENCY);
         }
 
@@ -591,7 +659,14 @@ std::vector<double> ControlLayer::saturateTorques(const std::vector<double>& req
 	return req;
 }
 
+std::vector<double> ControlLayer::getPublishedDesiredPosition(){
+	return this->writer_control_signal.msg.joints_position();
+}
+
+std::vector<double> ControlLayer::getPublishedDesiredVelocity(){
+	return this->writer_control_signal.msg.joints_velocity();
+}
+
 std::vector<double> ControlLayer::getPublishedDesiredTorques(){
-	// std::lock_guard<std::mutex>	lock(this->last_published_desired_torquesmutex);
-	return this->writer_control_signal.msg.desired_torques();
-}
+	return this->writer_control_signal.msg.joints_torques();
+}

modules/log/include/dls2/log/event_config.hpp

@@ -11,6 +11,7 @@ enum class EventID : uint8_t {
     WRONG_PROCESS_FREQUENCY,
     MISSING_INPUT,
     CPU_USAGE_TOO_HIGH,
+    CPU_TEMP_TOO_HIGH,
     MEM_USAGE_TOO_HIGH,
     INPUTS_NOT_SYNCHRONIZED,
     WRONG_INPUT_FREQUENCY,

modules/log/include/dls2/log/event_logger.hpp

@@ -18,6 +18,7 @@
 #include <cstddef>
 #include <ctime>
 #include <cstdio>
+#include <mutex>
 
 namespace dls
 {
@@ -87,6 +88,7 @@ namespace dls
 			std::vector<dls2_interface::msg::EventLog> readEvents(long int& idx_read);
 
 			boost::circular_buffer<dls2_interface::msg::EventLog> event_buffer_;
+			mutable std::mutex event_buffer_mutex_;
 
 		private:
 			const std::string name_;

modules/log/src/event_logger.cpp

@@ -126,6 +126,8 @@ EventListener::EventListener(const std::string &name)
 						// 		  	<< "\ncomponent: " << msg->component_name()
 						// 		  	<< "\nmessage: " << msg->msg()
 						// 		  	<< "\n###################" << std::endl;
+
+						std::lock_guard<std::mutex> lock(event_buffer_mutex_);
 						event_buffer_.push_back(*msg);
 						// }
 						unbounded_buffer_idx_++;
@@ -150,46 +152,43 @@ int EventListener::getNumOfMatches() const
 
 unsigned long long int EventListener::getUnboundedBufferIdx() const
 {
+	std::lock_guard<std::mutex> lock(event_buffer_mutex_);
 	return unbounded_buffer_idx_;
 }
 
 unsigned long int EventListener::getBufferMaxIdx() const
 {
+	std::lock_guard<std::mutex> lock(event_buffer_mutex_);
 	return event_buffer_.capacity()-1;
 }
 
 std::vector<dls2_interface::msg::EventLog> EventListener::readEvents(long int& idx_read)
 {
 	std::vector<dls2_interface::msg::EventLog> events;
-	events.clear();
 
-	idx_read = 0;
-	buffer_max_idx_ = getBufferMaxIdx();
+	std::lock_guard<std::mutex> lock(event_buffer_mutex_);
+	const long long latest_sequence_id = unbounded_buffer_idx_;
+	if (latest_sequence_id < 0 || latest_sequence_id < idx_read) {
+		return events;
+	}
 
-	long int idx_buffer = getUnboundedBufferIdx();
-	if(idx_buffer >= idx_read)
-	{
-		// mapping unbounded indexes in bounded indexes
-		long int delta = idx_buffer - idx_read;
-		if(delta > buffer_max_idx_){
-			idx_read = 0;
-			idx_buffer = buffer_max_idx_;
-		}
-		else if (idx_buffer >= buffer_max_idx_)
-		{
-			idx_read = buffer_max_idx_ - delta;
-			idx_buffer = idx_read + delta;
-		}
-
-		// read values
-		for(long int i = idx_read; i <= idx_buffer; ++i)
-		{
-			events.push_back(event_buffer_[i]);
-		}
-
-		// update read index
-		idx_read = idx_buffer + 1;
+	const long long oldest_retained_sequence_id =
+		latest_sequence_id - static_cast<long long>(event_buffer_.size()) + 1;
+	long long first_sequence_id = idx_read;
+	if (first_sequence_id < oldest_retained_sequence_id) {
+		first_sequence_id = oldest_retained_sequence_id;
 	}
 
+	events.reserve(static_cast<size_t>(latest_sequence_id - first_sequence_id + 1));
+	for (long long sequence_id = first_sequence_id;
+		 sequence_id <= latest_sequence_id;
+		 ++sequence_id) {
+		const auto buffer_index = static_cast<size_t>(
+			sequence_id - oldest_retained_sequence_id);
+		events.push_back(event_buffer_[buffer_index]);
+	}
+
+	idx_read = static_cast<long int>(latest_sequence_id + 1);
+
 	return events;
 }

modules/plugin/src/periodic_app_plugin.cpp

@@ -44,7 +44,8 @@ namespace dls
 
 		if(this->safety_layer_config_->enable_wrong_sequence_id){
 			for(size_t i = 0; i < input_info.size(); ++i){
-				if(input_info.at(i).missed_sequence_ids != 0){
+				if(input_info.at(i).missed_sequence_ids != 0 
+					&& input_info.at(i).topic_name != this->safety_layer_config_->process_status_topic){
 					this->robust_event_notifier.notify(
 						EventID::WRONG_SEQUENCE_ID,
 						EventSeverity::WARNING,

modules/plugin/test/CMakeLists.txt

@@ -12,6 +12,7 @@ target_link_libraries(${PLUGIN_NAME}
 	dls_signal
 	dls_messages
 	dls_topics
+	dls_controller
 	periodic_app_plugin
 )
 target_include_directories(${PLUGIN_NAME}

modules/plugin/test/include/hello_world_plugin.hpp

@@ -7,6 +7,7 @@
 #include "dls2/signal/reader.hpp"
 #include "dls2/signal/writer.hpp"
 // ******************************************
+#include "dls2/controller/control_modes.hpp"
 
 // plugin loaded at run-time
 class HelloWorldPlugin : public dls::PeriodicAppPlugin