lee_position_controller_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  *
00008  * Licensed under the Apache License, Version 2.0 (the "License");
00009  * you may not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  *     http://www.apache.org/licenses/LICENSE-2.0
00013 
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an "AS IS" BASIS,
00016  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
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   // Read parameters from rosparam.
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   // Clear all pending commands.
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   // Clear all pending commands.
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   // We can trigger the first command immediately.
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   // Todo(ffurrer): Do this in the conversions header.
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 }


rotors_control
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:38