Skip to content

Clean-up of main repo 5 #694

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
johanneskbl wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Clean-up of main repo 5 #694

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
134 changes: 66 additions & 68 deletions aerial_robot_control/include/aerial_robot_control/control/base/base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,30 +35,29 @@

#pragma once


#include <aerial_robot_control/flight_navigation.h>
#include <aerial_robot_estimation/state_estimation.h>
#include <aerial_robot_model/model/aerial_robot_model.h>
#include <ros/ros.h>
#include <spinal/PwmInfo.h>
#include <spinal/UavInfo.h>
#include "aerial_robot_control/flight_navigation.h"
#include "aerial_robot_estimation/state_estimation.h"
#include "aerial_robot_model/model/aerial_robot_model.h"
#include "spinal/PwmInfo.h"
#include "spinal/UavInfo.h"

namespace aerial_robot_control
{
class ControlBase
{
public:
ControlBase(): control_timestamp_(-1), activate_timestamp_(0)
{}
ControlBase(): control_timestamp_(-1), activate_timestamp_(0){}
virtual ~ControlBase() = default;

virtual ~ControlBase(){}
void virtual initialize(ros::NodeHandle nh,
virtual void initialize(ros::NodeHandle nh,
ros::NodeHandle nhp,
boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
boost::shared_ptr<aerial_robot_estimation::StateEstimator> estimator,
boost::shared_ptr<aerial_robot_navigation::BaseNavigator> navigator,
double ctrl_loop_du)
{
// Base implementation for the control initialization
nh_ = nh;
nhp_ = nhp;
motor_info_pub_ = nh_.advertise<spinal::PwmInfo>("motor_info", 10);
Expand All @@ -74,7 +73,7 @@ namespace aerial_robot_control
estimate_mode_ = estimator_->getEstimateMode();

getParam<bool>(nhp_, "param_verbose", param_verbose_, false);
getParam<int>(nh_, "uav_model", uav_model_, 0); //0: DRONE
getParam<int>(nh_, "uav_model", uav_model_, 0); // 0: DRONE

ros::NodeHandle control_nh(nh_, "controller");
getParam<bool>(control_nh, "control_verbose", control_verbose_, false);
Expand All @@ -89,77 +88,77 @@ namespace aerial_robot_control

int vel_ref_num;
getParam<int>(motor_nh, "vel_ref_num", vel_ref_num, 0);
motor_info_.resize(vel_ref_num);
motor_info_.resize(vel_ref_num); // Update size of vector
// Get motor voltage and max thrust for each rotor
for(int i = 0; i < vel_ref_num; i++)
{
std::stringstream ss;
ss << i + 1;
ros::NodeHandle motor_nh_ref(motor_nh, "ref" + ss.str());
double any_val; // Gets overritten to avoid multiple allocations
getParam<double>(motor_nh_ref, "voltage", any_val, 0.0);
motor_info_[i].voltage = any_val;
getParam<double>(motor_nh_ref, "max_thrust", any_val, 0.0);
motor_info_[i].max_thrust = any_val;

// Hardcoded: up to 4 dimensions
for(int j = 0; j < 5; j++)
{
std::stringstream ss;
ss << i + 1;
double val;
ros::NodeHandle nh(motor_nh, "ref" + ss.str());
getParam<double>(nh, "voltage", val, 0);
motor_info_[i].voltage = val;
nh.param("max_thrust", val, 0.0);
motor_info_[i].max_thrust = val;

/* hardcode: up to 4 dimension */
for(int j = 0; j < 5; j++)
{
std::stringstream ss2;
ss2 << j;
getParam<double>(nh, "polynominal" + ss2.str(), val, 0);
motor_info_[i].polynominal[j] = val;
}
std::stringstream ss2;
ss2 << j;
getParam<double>(motor_nh_ref, "polynominal" + ss2.str(), any_val, 0);
motor_info_[i].polynominal[j] = any_val;
}
}
}

virtual bool update()
{
// Base implementation for the control update
if(navigator_->getNaviState() == aerial_robot_navigation::START_STATE) activate();
if(navigator_->getNaviState() == aerial_robot_navigation::ARM_OFF_STATE && control_timestamp_ > 0)
{
reset();
}
{
reset();
}

if (control_timestamp_ < 0)
{
if (navigator_->getNaviState() == aerial_robot_navigation::TAKEOFF_STATE)
{
if (navigator_->getNaviState() == aerial_robot_navigation::TAKEOFF_STATE)
{
reset();
control_timestamp_ = ros::Time::now().toSec();

}
else return false;
reset();
control_timestamp_ = ros::Time::now().toSec();
}
else return false;
}

return true;
}

virtual void activate()
{
/* motor related info */
/* initialize setting */
if(ros::Time::now().toSec() - activate_timestamp_ > 0.1)
{
/* send motor and uav info to uav, about 10Hz */
spinal::PwmInfo motor_info_msg;
motor_info_msg.max_pwm = max_pwm_;
motor_info_msg.min_pwm = min_pwm_;
motor_info_msg.min_thrust = min_thrust_;
motor_info_msg.force_landing_thrust = force_landing_thrust_;
motor_info_msg.pwm_conversion_mode = pwm_conversion_mode_;
motor_info_msg.motor_info.resize(0);
for(int i = 0; i < motor_info_.size(); i++)
motor_info_msg.motor_info.push_back(motor_info_[i]);
motor_info_pub_.publish(motor_info_msg);

spinal::UavInfo uav_info_msg;
uav_info_msg.motor_num = motor_num_;
uav_info_msg.uav_model = uav_model_;
uav_info_pub_.publish(uav_info_msg);

activate_timestamp_ = ros::Time::now().toSec();

}
// Base implementation for the activation of the low-level motor control
// Send basic motor related info and (re)initialize settings
if(ros::Time::now().toSec() - activate_timestamp_ > 0.1)
{
// Send motor and UAV info to UAV in roughly 10Hz
spinal::PwmInfo motor_info_msg;
motor_info_msg.max_pwm = max_pwm_;
motor_info_msg.min_pwm = min_pwm_;
motor_info_msg.min_thrust = min_thrust_;
motor_info_msg.force_landing_thrust = force_landing_thrust_;
motor_info_msg.pwm_conversion_mode = pwm_conversion_mode_;
motor_info_msg.motor_info.resize(0);
for(int i = 0; i < motor_info_.size(); i++)
motor_info_msg.motor_info.push_back(motor_info_[i]);
motor_info_pub_.publish(motor_info_msg);

spinal::UavInfo uav_info_msg;
uav_info_msg.motor_num = motor_num_;
uav_info_msg.uav_model = uav_model_;
uav_info_pub_.publish(uav_info_msg);

activate_timestamp_ = ros::Time::now().toSec();
}
reset();
}

Expand All @@ -171,8 +170,8 @@ namespace aerial_robot_control
protected:
ros::NodeHandle nh_;
ros::NodeHandle nhp_;
ros::Publisher motor_info_pub_;
ros::Publisher uav_info_pub_;
ros::Publisher motor_info_pub_;
ros::Publisher uav_info_pub_;

boost::shared_ptr<aerial_robot_model::RobotModel> robot_model_;
boost::shared_ptr<aerial_robot_estimation::StateEstimator> estimator_;
Expand All @@ -186,11 +185,11 @@ namespace aerial_robot_control
int uav_model_;

double m_f_rate_;
double max_pwm_, min_pwm_;
double min_pwm_, max_pwm_;
double min_thrust_;
std::vector<spinal::MotorInfo> motor_info_;

double force_landing_thrust_; //pwm
double force_landing_thrust_; // pwm
int pwm_conversion_mode_;

int estimate_mode_;
Expand All @@ -205,5 +204,4 @@ namespace aerial_robot_control
ROS_INFO_STREAM("[" << nh.getNamespace() << "] " << param_name << ": " << param);
}
};

};
31 changes: 16 additions & 15 deletions aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,48 +35,51 @@

#pragma once

#include <aerial_robot_control/control/base/base.h>
#include <aerial_robot_control/control/utils/pid.h>
#include <aerial_robot_control/PIDConfig.h>
#include <aerial_robot_msgs/DynamicReconfigureLevels.h>
#include <aerial_robot_msgs/PoseControlPid.h>
#include <angles/angles.h>
#include <dynamic_reconfigure/server.h>
#include "aerial_robot_control/control/base/base.h"
#include "aerial_robot_control/control/utils/pid.h"
#include "aerial_robot_control/PIDConfig.h"
#include "aerial_robot_msgs/DynamicReconfigureLevels.h"
#include "aerial_robot_msgs/PoseControlPid.h"
#include "dynamic_reconfigure/server.h"

/* Allows parameter configuration at runtime using ROS's PIDConfig type
* which is autogenerated and customized for the aerial_robot_control package
* by the dynamic_reconfigure package while building */
using PidControlDynamicConfig = dynamic_reconfigure::Server<aerial_robot_control::PIDConfig>;

namespace aerial_robot_control
{
enum
{
X, Y, Z, ROLL, PITCH, YAW,
};
{
X, Y, Z, ROLL, PITCH, YAW,
};

class PoseLinearController: public ControlBase
{
public:
PoseLinearController();
virtual ~PoseLinearController() = default;

void virtual initialize(ros::NodeHandle nh,
ros::NodeHandle nhp,
boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
boost::shared_ptr<aerial_robot_estimation::StateEstimator> estimator,
boost::shared_ptr<aerial_robot_navigation::BaseNavigator> navigator,
double ctrl_loop_du) override;

virtual bool update() override;
virtual void reset() override;

protected:
ros::Publisher pid_pub_;

std::vector<PID> pid_controllers_;
std::vector<boost::shared_ptr<PidControlDynamicConfig> > pid_reconf_servers_;
std::vector<PID> pid_controllers_; // Vector of independent PID controllers for each coordinate
std::vector<boost::shared_ptr<PidControlDynamicConfig>> pid_reconf_servers_;
aerial_robot_msgs::PoseControlPid pid_msg_;

bool need_yaw_d_control_;
bool start_rp_integration_;
double start_rp_integration_height_;
double start_rp_integration_height_; // Start integration inside PID controller of roll and pitch at [this height + estimator_->getLandingHeight()]

double landing_err_z_;
double safe_landing_height_;
Expand All @@ -91,8 +94,6 @@ namespace aerial_robot_control
virtual void controlCore();
virtual void sendCmd();


void cfgPidCallback(aerial_robot_control::PIDConfig &config, uint32_t level, std::vector<int> controller_indices);
};

};
24 changes: 11 additions & 13 deletions aerial_robot_control/include/aerial_robot_control/control/fully_actuated_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,11 +34,11 @@
*********************************************************************/
#pragma once

#include <aerial_robot_msgs/WrenchAllocationMatrix.h>
#include <aerial_robot_control/control/base/pose_linear_controller.h>
#include <spinal/FourAxisCommand.h>
#include <spinal/RollPitchYawTerms.h>
#include <spinal/TorqueAllocationMatrixInv.h>
#include "aerial_robot_msgs/WrenchAllocationMatrix.h"
#include "aerial_robot_control/control/base/pose_linear_controller.h"
#include "spinal/FourAxisCommand.h"
#include "spinal/RollPitchYawTerms.h"
#include "spinal/TorqueAllocationMatrixInv.h"

using boost::algorithm::clamp;

Expand All @@ -62,12 +62,12 @@ namespace aerial_robot_control
virtual void sendCmd() override;

private:
ros::Publisher flight_cmd_pub_; //for spinal
ros::Publisher rpy_gain_pub_; //for spinal
ros::Publisher torque_allocation_matrix_inv_pub_; //for spinal
ros::Publisher flight_cmd_pub_; // For spinal
ros::Publisher rpy_gain_pub_; // For spinal
ros::Publisher torque_allocation_matrix_inv_pub_; // For spinal
double torque_allocation_matrix_inv_pub_stamp_;
ros::Publisher wrench_allocation_matrix_pub_; //for debug
ros::Publisher wrench_allocation_matrix_inv_pub_; //for debug
ros::Publisher wrench_allocation_matrix_pub_; // For debug
ros::Publisher wrench_allocation_matrix_inv_pub_; // For debug
double wrench_allocation_matrix_pub_stamp_;

Eigen::MatrixXd q_mat_;
Expand All @@ -84,7 +84,5 @@ namespace aerial_robot_control

void sendFourAxisCommand();
void sendTorqueAllocationMatrixInv();


};
} //namespace aerial_robot_control
};
20 changes: 9 additions & 11 deletions aerial_robot_control/include/aerial_robot_control/control/under_actuated_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,10 @@
*********************************************************************/
#pragma once

#include <aerial_robot_control/control/base/pose_linear_controller.h>
#include <spinal/FourAxisCommand.h>
#include <spinal/RollPitchYawTerms.h>
#include <spinal/TorqueAllocationMatrixInv.h>
#include "aerial_robot_control/control/base/pose_linear_controller.h"
#include "spinal/FourAxisCommand.h"
#include "spinal/RollPitchYawTerms.h"
#include "spinal/TorqueAllocationMatrixInv.h"

using boost::algorithm::clamp;

Expand All @@ -58,15 +58,15 @@ namespace aerial_robot_control
virtual void reset() override;

protected:
ros::Publisher flight_cmd_pub_; //for spinal
ros::Publisher rpy_gain_pub_; //for spinal
ros::Publisher torque_allocation_matrix_inv_pub_; //for spinal
ros::Publisher rpy_gain_pub_; // For spinal
ros::Publisher flight_cmd_pub_; // For spinal
ros::Publisher torque_allocation_matrix_inv_pub_; // For spinal
double torque_allocation_matrix_inv_pub_stamp_;

Eigen::MatrixXd q_mat_;
Eigen::MatrixXd q_mat_inv_;

double target_roll_, target_pitch_; // under-actuated
double target_roll_, target_pitch_; // Under-actuated
double candidate_yaw_term_;
std::vector<float> target_base_thrust_;

Expand All @@ -82,7 +82,5 @@ namespace aerial_robot_control
virtual void sendCmd() override;
virtual void sendFourAxisCommand();
virtual void sendTorqueAllocationMatrixInv();


};
} //namespace aerial_robot_control
};
Loading