00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <ros/ros.h>
00022 #include <mav_msgs/default_topics.h>
00023
00024 #include "lee_position_controller_node.h"
00025
00026 #include "rotors_control/parameters_ros.h"
00027
00028 namespace rotors_control {
00029
00030 LeePositionControllerNode::LeePositionControllerNode(
00031 const ros::NodeHandle& nh, const ros::NodeHandle& private_nh)
00032 :nh_(nh),
00033 private_nh_(private_nh){
00034 InitializeParams();
00035
00036 cmd_pose_sub_ = nh_.subscribe(
00037 mav_msgs::default_topics::COMMAND_POSE, 1,
00038 &LeePositionControllerNode::CommandPoseCallback, this);
00039
00040 cmd_multi_dof_joint_trajectory_sub_ = nh_.subscribe(
00041 mav_msgs::default_topics::COMMAND_TRAJECTORY, 1,
00042 &LeePositionControllerNode::MultiDofJointTrajectoryCallback, this);
00043
00044 odometry_sub_ = nh_.subscribe(mav_msgs::default_topics::ODOMETRY, 1,
00045 &LeePositionControllerNode::OdometryCallback, this);
00046
00047 motor_velocity_reference_pub_ = nh_.advertise<mav_msgs::Actuators>(
00048 mav_msgs::default_topics::COMMAND_ACTUATORS, 1);
00049
00050 command_timer_ = nh_.createTimer(ros::Duration(0), &LeePositionControllerNode::TimedCommandCallback, this,
00051 true, false);
00052 }
00053
00054 LeePositionControllerNode::~LeePositionControllerNode() { }
00055
00056 void LeePositionControllerNode::InitializeParams() {
00057
00058
00059 GetRosParameter(private_nh_, "position_gain/x",
00060 lee_position_controller_.controller_parameters_.position_gain_.x(),
00061 &lee_position_controller_.controller_parameters_.position_gain_.x());
00062 GetRosParameter(private_nh_, "position_gain/y",
00063 lee_position_controller_.controller_parameters_.position_gain_.y(),
00064 &lee_position_controller_.controller_parameters_.position_gain_.y());
00065 GetRosParameter(private_nh_, "position_gain/z",
00066 lee_position_controller_.controller_parameters_.position_gain_.z(),
00067 &lee_position_controller_.controller_parameters_.position_gain_.z());
00068 GetRosParameter(private_nh_, "velocity_gain/x",
00069 lee_position_controller_.controller_parameters_.velocity_gain_.x(),
00070 &lee_position_controller_.controller_parameters_.velocity_gain_.x());
00071 GetRosParameter(private_nh_, "velocity_gain/y",
00072 lee_position_controller_.controller_parameters_.velocity_gain_.y(),
00073 &lee_position_controller_.controller_parameters_.velocity_gain_.y());
00074 GetRosParameter(private_nh_, "velocity_gain/z",
00075 lee_position_controller_.controller_parameters_.velocity_gain_.z(),
00076 &lee_position_controller_.controller_parameters_.velocity_gain_.z());
00077 GetRosParameter(private_nh_, "attitude_gain/x",
00078 lee_position_controller_.controller_parameters_.attitude_gain_.x(),
00079 &lee_position_controller_.controller_parameters_.attitude_gain_.x());
00080 GetRosParameter(private_nh_, "attitude_gain/y",
00081 lee_position_controller_.controller_parameters_.attitude_gain_.y(),
00082 &lee_position_controller_.controller_parameters_.attitude_gain_.y());
00083 GetRosParameter(private_nh_, "attitude_gain/z",
00084 lee_position_controller_.controller_parameters_.attitude_gain_.z(),
00085 &lee_position_controller_.controller_parameters_.attitude_gain_.z());
00086 GetRosParameter(private_nh_, "angular_rate_gain/x",
00087 lee_position_controller_.controller_parameters_.angular_rate_gain_.x(),
00088 &lee_position_controller_.controller_parameters_.angular_rate_gain_.x());
00089 GetRosParameter(private_nh_, "angular_rate_gain/y",
00090 lee_position_controller_.controller_parameters_.angular_rate_gain_.y(),
00091 &lee_position_controller_.controller_parameters_.angular_rate_gain_.y());
00092 GetRosParameter(private_nh_, "angular_rate_gain/z",
00093 lee_position_controller_.controller_parameters_.angular_rate_gain_.z(),
00094 &lee_position_controller_.controller_parameters_.angular_rate_gain_.z());
00095 GetVehicleParameters(private_nh_, &lee_position_controller_.vehicle_parameters_);
00096 lee_position_controller_.InitializeParameters();
00097 }
00098 void LeePositionControllerNode::Publish() {
00099 }
00100
00101 void LeePositionControllerNode::CommandPoseCallback(
00102 const geometry_msgs::PoseStampedConstPtr& pose_msg) {
00103
00104 command_timer_.stop();
00105 commands_.clear();
00106 command_waiting_times_.clear();
00107
00108 mav_msgs::EigenTrajectoryPoint eigen_reference;
00109 mav_msgs::eigenTrajectoryPointFromPoseMsg(*pose_msg, &eigen_reference);
00110 commands_.push_front(eigen_reference);
00111
00112 lee_position_controller_.SetTrajectoryPoint(commands_.front());
00113 commands_.pop_front();
00114 }
00115
00116 void LeePositionControllerNode::MultiDofJointTrajectoryCallback(
00117 const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& msg) {
00118
00119 command_timer_.stop();
00120 commands_.clear();
00121 command_waiting_times_.clear();
00122
00123 const size_t n_commands = msg->points.size();
00124
00125 if(n_commands < 1){
00126 ROS_WARN_STREAM("Got MultiDOFJointTrajectory message, but message has no points.");
00127 return;
00128 }
00129
00130 mav_msgs::EigenTrajectoryPoint eigen_reference;
00131 mav_msgs::eigenTrajectoryPointFromMsg(msg->points.front(), &eigen_reference);
00132 commands_.push_front(eigen_reference);
00133
00134 for (size_t i = 1; i < n_commands; ++i) {
00135 const trajectory_msgs::MultiDOFJointTrajectoryPoint& reference_before = msg->points[i-1];
00136 const trajectory_msgs::MultiDOFJointTrajectoryPoint& current_reference = msg->points[i];
00137
00138 mav_msgs::eigenTrajectoryPointFromMsg(current_reference, &eigen_reference);
00139
00140 commands_.push_back(eigen_reference);
00141 command_waiting_times_.push_back(current_reference.time_from_start - reference_before.time_from_start);
00142 }
00143
00144
00145 lee_position_controller_.SetTrajectoryPoint(commands_.front());
00146 commands_.pop_front();
00147
00148 if (n_commands > 1) {
00149 command_timer_.setPeriod(command_waiting_times_.front());
00150 command_waiting_times_.pop_front();
00151 command_timer_.start();
00152 }
00153 }
00154
00155 void LeePositionControllerNode::TimedCommandCallback(const ros::TimerEvent& e) {
00156
00157 if(commands_.empty()){
00158 ROS_WARN("Commands empty, this should not happen here");
00159 return;
00160 }
00161
00162 const mav_msgs::EigenTrajectoryPoint eigen_reference = commands_.front();
00163 lee_position_controller_.SetTrajectoryPoint(commands_.front());
00164 commands_.pop_front();
00165 command_timer_.stop();
00166 if(!command_waiting_times_.empty()){
00167 command_timer_.setPeriod(command_waiting_times_.front());
00168 command_waiting_times_.pop_front();
00169 command_timer_.start();
00170 }
00171 }
00172
00173 void LeePositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
00174
00175 ROS_INFO_ONCE("LeePositionController got first odometry message.");
00176
00177 EigenOdometry odometry;
00178 eigenOdometryFromMsg(odometry_msg, &odometry);
00179 lee_position_controller_.SetOdometry(odometry);
00180
00181 Eigen::VectorXd ref_rotor_velocities;
00182 lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
00183
00184
00185 mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);
00186
00187 actuator_msg->angular_velocities.clear();
00188 for (int i = 0; i < ref_rotor_velocities.size(); i++)
00189 actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
00190 actuator_msg->header.stamp = odometry_msg->header.stamp;
00191
00192 motor_velocity_reference_pub_.publish(actuator_msg);
00193 }
00194
00195 }
00196
00197 int main(int argc, char** argv) {
00198 ros::init(argc, argv, "lee_position_controller_node");
00199
00200 ros::NodeHandle nh;
00201 ros::NodeHandle private_nh("~");
00202 rotors_control::LeePositionControllerNode lee_position_controller_node(nh, private_nh);
00203
00204 ros::spin();
00205
00206 return 0;
00207 }