Skip to content

Commit 21753c1

Browse files
Jaeyoung-LimJaeyoung-Lim
authored
Merge pull request #77 from Jaeyoung-Lim/pr-controller-eval
Add evaluation node for controller performance measurements
2 parents 5b572c9 + 071e434 commit 21753c1

File tree

9 files changed

+341
-0
lines changed

9 files changed

+341
-0
lines changed

‎controller_benchmark/CMakeLists.txt‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(controller_benchmark)
3+
4+
include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS})
5+
6+
find_package(catkin_simple REQUIRED)
7+
catkin_simple(ALL_DEPS_REQUIRED)
8+
9+
add_definitions(-std=c++11)
10+
11+
#############
12+
# LIBRARIES #
13+
#############
14+
cs_add_library(${PROJECT_NAME}
15+
src/controller_benchmark.cpp
16+
src/error_measurer.cpp
17+
)
18+
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
19+
20+
############
21+
# BINARIES #
22+
############
23+
cs_add_executable(controller_benchmark_node
24+
src/controller_benchmark_node.cpp
25+
)
26+
target_link_libraries(controller_benchmark_node ${PROJECT_NAME} ${catkin_LIBRARIES})
27+
##########
28+
# EXPORT #
29+
##########
30+
cs_install()
31+
cs_export()

‎controller_benchmark/README.md‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
# controller_eval
2+
3+
This is a node for evaluating performance of different controllers.

‎controller_benchmark/include/controller_benchmark/controller_benchmark.h‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// July/2018, ETHZ, Jaeyoung Lim, [email protected]
2+
3+
#ifndef CONTROLLER_EVAL_H
4+
#define CONTROLLER_EVAL_H
5+
6+
#include <Eigen/Dense>
7+
8+
#include <ros/ros.h>
9+
#include <gazebo_msgs/ModelStates.h>
10+
#include <geometry_msgs/PoseStamped.h>
11+
#include <geometry_msgs/TwistStamped.h>
12+
#include <controller_benchmark/error_measurer.h>
13+
14+
class ControllerEvaluator
15+
{
16+
private:
17+
ros::NodeHandle nh_;
18+
ros::NodeHandle nh_private_;
19+
20+
ros::Subscriber mavposeSub_;
21+
ros::Subscriber mavtwistSub_;
22+
ros::Subscriber gzmavposeSub_;
23+
24+
std::string mav_name_;
25+
26+
Eigen::Vector3d mavPos_;
27+
Eigen::Vector3d mavVel_;
28+
Eigen::Vector3d mavRate_;
29+
Eigen::Vector4d mavAtt_;
30+
Eigen::Vector3d gt_mavPos_;
31+
Eigen::Vector3d gt_mavVel_;
32+
Eigen::Vector3d gt_mavRate_;
33+
Eigen::Vector4d gt_mavAtt_;
34+
Eigen::Vector3d error_pos_;
35+
Eigen::Vector3d error_vel_;
36+
37+
std::vector<ErrorMeasurer> error_measurer_;
38+
39+
void mavposeCallback(const geometry_msgs::PoseStamped& msg);
40+
void mavtwistCallback(const geometry_msgs::TwistStamped& msg);
41+
void gzmavposeCallback(const gazebo_msgs::ModelStates& msg);
42+
43+
public:
44+
ControllerEvaluator(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);
45+
virtual ~ ControllerEvaluator();
46+
};
47+
48+
49+
#endif

‎controller_benchmark/include/controller_benchmark/error_measurer.h‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
2+
// April/2019, ETHZ, Jaeyoung Lim, [email protected]
3+
4+
#ifndef CONTROLLER_EVAL_ERROR_MEASURER_H
5+
#define CONTROLLER_EVAL_ERROR_MEASURER_H
6+
7+
#include <ros/ros.h>
8+
#include <ros/subscribe_options.h>
9+
#include <Eigen/Dense>
10+
11+
class ErrorMeasurer
12+
{
13+
private:
14+
double mean_error_;
15+
double threshold_;
16+
double max_;
17+
double min_;
18+
double sum_error_;
19+
int N;
20+
int windowsamples_;
21+
int totalsamples_;
22+
std::vector<double> sample_error_;
23+
24+
public:
25+
ErrorMeasurer();
26+
virtual ~ ErrorMeasurer();
27+
void Add(double error);
28+
void GetMeanError(double &mean_error);
29+
int GetWindowSamples();
30+
};
31+
32+
#endif //CONTROLLER_EVAL_ERROR_MEASURER_H

‎controller_benchmark/launch/controller_benchmark.launch‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
<launch>
2+
<arg name="mav_name" default="iris"/>
3+
<arg name="fcu_url" default="udp://:[email protected]:14557"/>
4+
<arg name="gcs_url" default="" />
5+
<arg name="tgt_system" default="1" />
6+
<arg name="tgt_component" default="1" />
7+
<arg name="command_input" default="2" />
8+
<arg name="gazebo_simulation" default="true" />
9+
<arg name="visualization" default="true"/>
10+
<arg name="log_output" default="screen" />
11+
<arg name="fcu_protocol" default="v2.0" />
12+
<arg name="respawn_mavros" default="false" />
13+
14+
<node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
15+
<param name="mav_name" type="string" value="$(arg mav_name)" />
16+
<remap from="command/bodyrate_command" to="/mavros/setpoint_raw/attitude"/>
17+
<param name="ctrl_mode" value="$(arg command_input)" />
18+
<param name="enable_sim" value="$(arg gazebo_simulation)" />
19+
<param name="enable_gazebo_state" value="true"/>
20+
</node>
21+
22+
<node pkg="trajectory_publisher" type="trajectory_publisher" name="trajectory_publisher" output="screen">
23+
<param name="trajectory_type" value="1" />
24+
<param name="shape_omega" value="1.2" />
25+
<param name="initpos_z" value="2.0" />
26+
<param name="max_acc" value="10.0" />
27+
<param name="Kpos_x" value="8.0" />
28+
<param name="Kpos_y" value="8.0" />
29+
<param name="Kpos_z" value="30.0" />
30+
<param name="reference_type" value="2" />
31+
</node>
32+
33+
<node pkg="controller_benchmark" type="controller_benchmark_node" name="controller_benchmark" output="screen">
34+
</node>
35+
36+
<include file="$(find mavros)/launch/node.launch">
37+
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
38+
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
39+
40+
<arg name="fcu_url" value="$(arg fcu_url)" />
41+
<arg name="gcs_url" value="$(arg gcs_url)" />
42+
<arg name="tgt_system" value="$(arg tgt_system)" />
43+
<arg name="tgt_component" value="$(arg tgt_component)" />
44+
<arg name="log_output" value="$(arg log_output)" />
45+
<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
46+
<arg name="respawn_mavros" default="$(arg respawn_mavros)" />
47+
</include>
48+
49+
<include file="$(find px4)/launch/posix_sitl.launch">
50+
<arg name="vehicle" value="$(arg mav_name)"/>
51+
</include>
52+
53+
<group if="$(arg visualization)">
54+
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find geometric_controller)/launch/config_file.rviz" />
55+
</group>
56+
57+
</launch>

‎controller_benchmark/package.xml‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>controller_benchmark</name>
4+
<version>0.0.0</version>
5+
<description>Evaluator for controller performance benchmark</description>
6+
7+
<maintainer email="[email protected]">Jaeyoung Lim</maintainer>
8+
9+
<license>BSD</license>
10+
11+
12+
<buildtool_depend>catkin</buildtool_depend>
13+
<build_depend>roscpp</build_depend>
14+
<build_depend>rospy</build_depend>
15+
<build_depend>std_msgs</build_depend>
16+
<build_depend>catkin_simple</build_depend>
17+
<build_depend>nav_msgs</build_depend>
18+
<build_depend>geometry_msgs</build_depend>
19+
<build_depend>controller_msgs</build_depend>
20+
<build_depend>tf</build_depend>
21+
<build_depend>mavros_msgs</build_depend>
22+
<build_depend> eigen_catkin</build_depend>
23+
<run_depend>roscpp</run_depend>
24+
<run_depend>rospy</run_depend>
25+
<run_depend>catkin_simple</run_depend>
26+
<run_depend>std_msgs</run_depend>
27+
<run_depend>geometry_msgs</run_depend>
28+
<run_depend>controller_msgs</run_depend>
29+
<run_depend>nav_msgs</run_depend>
30+
<run_depend>tf</run_depend>
31+
<run_depend>mavros</run_depend>
32+
<run_depend>mavros_msgs</run_depend>
33+
34+
35+
<!-- The export tag contains other, unspecified, tags -->
36+
<export>
37+
<!-- Other tools can request additional information be placed here -->
38+
39+
</export>
40+
</package>

‎controller_benchmark/src/controller_benchmark.cpp‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
// April/2019, ETHZ, Jaeyoung Lim, [email protected]
2+
3+
#include "controller_benchmark/controller_benchmark.h"
4+
5+
//Constructor
6+
ControllerEvaluator::ControllerEvaluator(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private):
7+
nh_(nh),
8+
nh_private_(nh_private) {
9+
10+
mavposeSub_ = nh_.subscribe("/mavros/local_position/pose", 1, &ControllerEvaluator::mavposeCallback, this,ros::TransportHints().tcpNoDelay());
11+
mavtwistSub_ = nh_.subscribe("/mavros/local_position/velocity_local", 1, &ControllerEvaluator::mavtwistCallback, this,ros::TransportHints().tcpNoDelay());
12+
gzmavposeSub_ = nh_.subscribe("/gazebo/model_states", 1, &ControllerEvaluator::gzmavposeCallback, this, ros::TransportHints().tcpNoDelay());
13+
14+
error_measurer_.resize(3);
15+
}
16+
ControllerEvaluator::~ControllerEvaluator() {
17+
//Destructor
18+
}
19+
20+
void ControllerEvaluator::mavposeCallback(const geometry_msgs::PoseStamped& msg){
21+
mavPos_(0) = msg.pose.position.x;
22+
mavPos_(1) = msg.pose.position.y;
23+
mavPos_(2) = msg.pose.position.z;
24+
mavAtt_(0) = msg.pose.orientation.w;
25+
mavAtt_(1) = msg.pose.orientation.x;
26+
mavAtt_(2) = msg.pose.orientation.y;
27+
mavAtt_(3) = msg.pose.orientation.z;
28+
29+
error_pos_ = mavPos_ - gt_mavPos_;
30+
error_vel_ = mavVel_ - gt_mavVel_;
31+
32+
for(int i = 0; i < 3; i++){
33+
error_measurer_[i].Add(error_pos_(i));
34+
double error;
35+
error_measurer_[i].GetMeanError(error);
36+
}
37+
}
38+
39+
void ControllerEvaluator::mavtwistCallback(const geometry_msgs::TwistStamped& msg){
40+
41+
mavVel_(0) = msg.twist.linear.x;
42+
mavVel_(1) = msg.twist.linear.y;
43+
mavVel_(2) = msg.twist.linear.z;
44+
mavRate_(0) = msg.twist.angular.x;
45+
mavRate_(1) = msg.twist.angular.y;
46+
mavRate_(2) = msg.twist.angular.z;
47+
48+
}
49+
50+
void ControllerEvaluator::gzmavposeCallback(const gazebo_msgs::ModelStates& msg){
51+
for(int i = 0; i < msg.pose.size(); i++){
52+
if(msg.name[i] == mav_name_){
53+
gt_mavPos_(0) = msg.pose[i].position.x;
54+
gt_mavPos_(1) = msg.pose[i].position.y;
55+
gt_mavPos_(2) = msg.pose[i].position.z;
56+
gt_mavAtt_(0) = msg.pose[i].orientation.w;
57+
gt_mavAtt_(1) = msg.pose[i].orientation.x;
58+
gt_mavAtt_(2) = msg.pose[i].orientation.y;
59+
gt_mavAtt_(3) = msg.pose[i].orientation.z;
60+
gt_mavVel_(0) = msg.twist[i].linear.x;
61+
gt_mavVel_(1) = msg.twist[i].linear.y;
62+
gt_mavVel_(2) = msg.twist[i].linear.z;
63+
gt_mavRate_(0) = msg.twist[i].angular.x;
64+
gt_mavRate_(1) = msg.twist[i].angular.y;
65+
gt_mavRate_(2) = msg.twist[i].angular.z;
66+
break;
67+
}
68+
}
69+
}

‎controller_benchmark/src/controller_benchmark_node.cpp‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
// July/2018, ETHZ, Jaeyoung Lim, [email protected]
2+
3+
#include "controller_benchmark/controller_benchmark.h"
4+
5+
int main(int argc, char** argv) {
6+
ros::init(argc,argv,"geometric_controller");
7+
ros::NodeHandle nh("");
8+
ros::NodeHandle nh_private("~");
9+
10+
ControllerEvaluator geometricController(nh, nh_private);
11+
ros::spin();
12+
return 0;
13+
}

‎controller_benchmark/src/error_measurer.cpp‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
#include "controller_benchmark/error_measurer.h"
2+
3+
ErrorMeasurer::ErrorMeasurer() :
4+
totalsamples_(0),
5+
windowsamples_(0),
6+
N(50) {
7+
8+
sample_error_.resize(N);
9+
10+
for(int i = 0 ; i < N ; i++){
11+
sample_error_[i] = 0.0;
12+
}
13+
sum_error_ = 0.0;
14+
15+
}
16+
17+
ErrorMeasurer::~ErrorMeasurer() {
18+
//Destructor
19+
}
20+
21+
void ErrorMeasurer::Add(double error){
22+
//Accumulate error value
23+
if (windowsamples_ < N) {
24+
sample_error_[windowsamples_] = error;
25+
sum_error_ += error;
26+
sample_error_[windowsamples_] = error;
27+
28+
} else {
29+
double &oldest_error = sample_error_[windowsamples_ % N];
30+
31+
sum_error_ = sum_error_ - oldest_error + error;
32+
oldest_error = error;
33+
}
34+
windowsamples_++;
35+
36+
++totalsamples_;
37+
}
38+
39+
void ErrorMeasurer::GetMeanError(double &meanerror){
40+
if(windowsamples_ > N) meanerror = sum_error_ / N;
41+
else meanerror = sum_error_ / windowsamples_;
42+
}
43+
44+
int ErrorMeasurer::GetWindowSamples(){
45+
return windowsamples_;
46+
47+
}

0 commit comments

Comments
 (0)