$search
00001 /* 00002 * AscTec Autopilot Processor 00003 * Copyright (C) 2010, CCNY Robotics Lab 00004 * Ivan Dryanovski <ivan.dryanovski@gmail.com> 00005 * 00006 * http://robotics.ccny.cuny.edu 00007 * 00008 * This program is free software: you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation, either version 3 of the License, or 00011 * (at your option) any later version. 00012 * 00013 * This program is distributed in the hope that it will be useful, 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 * GNU General Public License for more details. 00017 * 00018 * You should have received a copy of the GNU General Public License 00019 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00020 */ 00021 00022 #include "asctec_proc/asctec_proc.h" 00023 00024 namespace asctec 00025 { 00026 00027 AsctecProc::AsctecProc(ros::NodeHandle nh, ros::NodeHandle nh_private): 00028 nh_(nh), 00029 nh_private_(nh_private) 00030 { 00031 ROS_INFO("Starting AsctecProc"); 00032 00033 ros::NodeHandle nh_rawdata (nh_, asctec::ROS_NAMESPACE); 00034 ros::NodeHandle nh_procdata (nh_, "mav"); 00035 00036 // **** get parameters 00037 00038 initializeParams(); 00039 00040 // **** initialize vaiables 00041 00042 motors_on_ = false; 00043 engaging_ = false; 00044 00045 ctrl_roll_ = 0; 00046 ctrl_pitch_ = 0; 00047 ctrl_yaw_ = 0; 00048 ctrl_thrust_ = 0; 00049 00050 assembleCtrlCommands(); 00051 00052 // *** register publishers 00053 00054 imu_publisher_ = nh_procdata.advertise<sensor_msgs::Imu>( 00055 "imu", 10); 00056 height_publisher_ = nh_procdata.advertise<mav_msgs::Height>( 00057 "pressure_height", 10); 00058 height_filtered_publisher_ = nh_procdata.advertise<mav_msgs::Height>( 00059 "pressure_height_filtered", 10); 00060 ctrl_input_publisher_ = nh_rawdata.advertise<asctec_msgs::CtrlInput>( 00061 asctec::CTRL_INPUT_TOPIC, 10); 00062 00063 // **** register subscribers 00064 00065 imu_calcdata_subscriber_ = nh_rawdata.subscribe( 00066 asctec::IMU_CALCDATA_TOPIC, 10, &AsctecProc::imuCalcDataCallback, this); 00067 ll_status_subscriber_ = nh_rawdata.subscribe( 00068 asctec::LL_STATUS_TOPIC, 5, &AsctecProc::llStatusCallback, this); 00069 00070 if (enable_ctrl_thrust_) 00071 { 00072 cmd_thrust_subscriber_ = nh_procdata.subscribe( 00073 "cmd_thrust", 1, &AsctecProc::cmdThrustCallback, this); 00074 } 00075 if (enable_ctrl_roll_) 00076 { 00077 cmd_roll_subscriber_ = nh_procdata.subscribe( 00078 "cmd_roll", 1, &AsctecProc::cmdRollCallback, this); 00079 } 00080 if (enable_ctrl_pitch_) 00081 { 00082 cmd_pitch_subscriber_ = nh_procdata.subscribe( 00083 "cmd_pitch", 1, &AsctecProc::cmdPitchCallback, this); 00084 } 00085 if (enable_ctrl_yaw_) 00086 { 00087 cmd_yaw_subscriber_ = nh_procdata.subscribe( 00088 "cmd_yaw", 5, &AsctecProc::cmdYawCallback, this); 00089 } 00090 00091 // **** services 00092 00093 if(enable_state_changes_) 00094 { 00095 set_motors_on_off_srv_ = nh_procdata.advertiseService( 00096 "setMotorsOnOff", &AsctecProc::setMotorsOnOff, this); 00097 } 00098 00099 get_motors_on_off_srv_ = nh_procdata.advertiseService( 00100 "getMotorsOnOff", &AsctecProc::getMotorsOnOff, this); 00101 } 00102 00103 AsctecProc::~AsctecProc() 00104 { 00105 ROS_INFO("Destroying AsctecProc"); 00106 00107 } 00108 00109 void AsctecProc::initializeParams() 00110 { 00111 if (!nh_private_.getParam ("enable_state_changes", enable_state_changes_)) 00112 enable_state_changes_ = false; 00113 00114 if (!nh_private_.getParam ("enable_ctrl_thrust", enable_ctrl_thrust_)) 00115 enable_ctrl_thrust_ = false; 00116 if (!nh_private_.getParam ("enable_ctrl_pitch", enable_ctrl_pitch_)) 00117 enable_ctrl_pitch_ = false; 00118 if (!nh_private_.getParam ("enable_ctrl_roll", enable_ctrl_roll_)) 00119 enable_ctrl_roll_ = false; 00120 if (!nh_private_.getParam ("enable_ctrl_yaw", enable_ctrl_yaw_)) 00121 enable_ctrl_yaw_ = false; 00122 00123 if (!nh_private_.getParam ("max_ctrl_thrust", max_ctrl_thrust_)) 00124 max_ctrl_thrust_ = 2200; 00125 if (!nh_private_.getParam ("max_ctrl_roll", max_ctrl_roll_)) 00126 max_ctrl_roll_ = 300; 00127 if (!nh_private_.getParam ("max_ctrl_pitch", max_ctrl_pitch_)) 00128 max_ctrl_pitch_ = 300; 00129 if (!nh_private_.getParam ("max_ctrl_yaw", max_ctrl_yaw_)) 00130 max_ctrl_yaw_ = 600; 00131 } 00132 00133 bool AsctecProc::setMotorsOnOff(mav_srvs::SetMotorsOnOff::Request &req, 00134 mav_srvs::SetMotorsOnOff::Response &res) 00135 { 00136 state_mutex_.lock(); 00137 engaging_ = true; 00138 00139 if (req.on && !motors_on_) 00140 { 00141 ctrl_roll_ = 0; 00142 ctrl_pitch_ = 0; 00143 ctrl_yaw_ = 0; 00144 ctrl_thrust_ = 0; 00145 startMotors(); 00146 } 00147 else 00148 { 00149 stopMotors(); 00150 } 00151 00152 engaging_ = false; 00153 state_mutex_.unlock(); 00154 00155 return (req.on == motors_on_); 00156 } 00157 00158 bool AsctecProc::getMotorsOnOff(mav_srvs::GetMotorsOnOff::Request &req, 00159 mav_srvs::GetMotorsOnOff::Response &res) 00160 { 00161 state_mutex_.lock(); 00162 res.on = motors_on_; 00163 state_mutex_.unlock(); 00164 00165 return true; 00166 } 00167 00168 void AsctecProc::llStatusCallback (const asctec_msgs::LLStatusPtr& ll_status_msg) 00169 { 00170 // save the state of the motors 00171 motors_on_ = ll_status_msg->flying; 00172 } 00173 00174 void AsctecProc::cmdRollCallback(const std_msgs::Float64ConstPtr& cmd_roll_msg) 00175 { 00176 if (!motors_on_ || engaging_) return; 00177 00178 state_mutex_.lock(); 00179 00180 // translate from cmd_roll [-1.0 to 1.0] to ctrl_roll [-2047 .. 2047], 00181 ctrl_roll_ = (int)(cmd_roll_msg->data * asctec::ROS_TO_ASC_ROLL); 00182 00183 ROS_INFO ("cmd_roll received: %f (%d)", cmd_roll_msg->data, ctrl_roll_); 00184 00185 // limit min/max output 00186 if (ctrl_roll_ > max_ctrl_roll_) 00187 { 00188 ROS_WARN("ctrl_roll of %d too big, clamping to %d!", ctrl_roll_, max_ctrl_roll_); 00189 ctrl_roll_ = max_ctrl_roll_; 00190 } 00191 else if (ctrl_roll_ < -max_ctrl_roll_) 00192 { 00193 ROS_WARN("ctrl_roll of %d too small, clamping to -%d!", ctrl_roll_, -max_ctrl_roll_); 00194 ctrl_roll_ = -max_ctrl_roll_; 00195 } 00196 00197 publishCtrlInputMsg(); 00198 00199 state_mutex_.unlock(); 00200 } 00201 00202 void AsctecProc::cmdPitchCallback(const std_msgs::Float64ConstPtr& cmd_pitch_msg) 00203 { 00204 if (!motors_on_ || engaging_) return; 00205 00206 state_mutex_.lock(); 00207 00208 // translate from cmd_pitch [-1.0 to 1.0] to ctrl_pitch [-2047 .. 2047], 00209 ctrl_pitch_ = (int)(cmd_pitch_msg->data * asctec::ROS_TO_ASC_PITCH); 00210 00211 ROS_DEBUG ("cmd_pitch received: %f (%d)", cmd_pitch_msg->data, ctrl_pitch_); 00212 00213 // limit min/max output 00214 if (ctrl_pitch_ > max_ctrl_pitch_) 00215 { 00216 ROS_WARN("ctrl_pitch of %d too big, clamping to %d!", ctrl_pitch_, max_ctrl_pitch_); 00217 ctrl_pitch_ = max_ctrl_pitch_; 00218 } 00219 else if (ctrl_pitch_ < -max_ctrl_pitch_) 00220 { 00221 ROS_WARN("ctrl_pitch of %d too small, clamping to -%d!", ctrl_pitch_, -max_ctrl_pitch_); 00222 ctrl_pitch_ = -max_ctrl_pitch_; 00223 } 00224 00225 publishCtrlInputMsg(); 00226 00227 state_mutex_.unlock(); 00228 } 00229 00230 void AsctecProc::cmdYawCallback(const std_msgs::Float64ConstPtr& cmd_yaw_rate_msg) 00231 { 00232 if (!motors_on_ || engaging_) return; 00233 00234 state_mutex_.lock(); 00235 00236 // translate from cmd_yaw [rad/s] to ctrl_yaw [-2047 .. 2047], 00237 ctrl_yaw_ = (int)(cmd_yaw_rate_msg->data * asctec::ROS_TO_ASC_YAW_RATE); 00238 00239 ROS_DEBUG ("cmd_yaw received: %f (%d)", cmd_yaw_rate_msg->data, ctrl_yaw_); 00240 00241 // limit min/max output 00242 if (ctrl_yaw_ > max_ctrl_yaw_) 00243 { 00244 ROS_WARN("ctrl_yaw of %d too big, clamping to %d!", ctrl_yaw_, max_ctrl_yaw_); 00245 ctrl_yaw_ = max_ctrl_yaw_; 00246 } 00247 else if (ctrl_yaw_ < -max_ctrl_yaw_) 00248 { 00249 ROS_WARN("ctrl_yaw of %d too small, clamping to -%d!", ctrl_yaw_, -max_ctrl_yaw_); 00250 ctrl_yaw_ = -max_ctrl_yaw_; 00251 } 00252 00253 publishCtrlInputMsg(); 00254 00255 state_mutex_.unlock(); 00256 } 00257 00258 void AsctecProc::cmdThrustCallback(const std_msgs::Float64ConstPtr& cmd_thrust_msg) 00259 { 00260 if (!motors_on_ || engaging_) return; 00261 00262 state_mutex_.lock(); 00263 00264 // translate from cmd_thrust [0.0 to 1.0] to ctrl_thrust [0 to 4095], 00265 ctrl_thrust_ = (int)(cmd_thrust_msg->data * asctec::ROS_TO_ASC_THRUST); 00266 00267 ROS_DEBUG ("cmd_thrust received: %f (%d)", cmd_thrust_msg->data, ctrl_thrust_); 00268 00269 // limit min-max output 00270 if (ctrl_thrust_ > max_ctrl_thrust_) 00271 { 00272 ROS_WARN("ctrl_thrust of %d too big, clamping to %d!", ctrl_thrust_, max_ctrl_thrust_); 00273 ctrl_thrust_ = max_ctrl_thrust_; 00274 } 00275 else if (ctrl_thrust_ < 0) 00276 { 00277 ROS_WARN("ctrl_thrust of %d too small, clamping to 0!", ctrl_thrust_); 00278 ctrl_thrust_ = 0; 00279 } 00280 00281 publishCtrlInputMsg(); 00282 00283 state_mutex_.unlock(); 00284 } 00285 00286 void AsctecProc::imuCalcDataCallback(const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg) 00287 { 00288 // publish imu message 00289 sensor_msgs::ImuPtr imu_msg; 00290 imu_msg = boost::make_shared<sensor_msgs::Imu>(); 00291 createImuMsg (imu_calcdata_msg, imu_msg); 00292 imu_publisher_.publish(imu_msg); 00293 00294 // publish unfiltered height message 00295 mav_msgs::HeightPtr height_msg; 00296 height_msg = boost::make_shared<mav_msgs::Height>(); 00297 createHeightMsg (imu_calcdata_msg, height_msg); 00298 height_publisher_.publish(height_msg); 00299 00300 // publish filtered height message 00301 mav_msgs::HeightPtr height_filtered_msg; 00302 height_filtered_msg = boost::make_shared<mav_msgs::Height>(); 00303 createHeightFilteredMsg (imu_calcdata_msg, height_filtered_msg); 00304 height_filtered_publisher_.publish(height_filtered_msg); 00305 } 00306 00307 void AsctecProc::createHeightMsg(const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg, 00308 mav_msgs::HeightPtr& height_msg) 00309 { 00310 // set header info 00311 height_msg->header.stamp = imu_calcdata_msg->header.stamp; 00312 height_msg->header.frame_id = "imu"; // the frame seems arbitrary here 00313 00314 height_msg->height = imu_calcdata_msg->height_reference * asctec::ASC_TO_ROS_HEIGHT; 00315 height_msg->climb = imu_calcdata_msg->dheight_reference * asctec::ASC_TO_ROS_HEIGHT; 00316 } 00317 00318 void AsctecProc::createHeightFilteredMsg(const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg, 00319 mav_msgs::HeightPtr& height_filtered_msg) 00320 { 00321 // set header info 00322 height_filtered_msg->header.stamp = imu_calcdata_msg->header.stamp; 00323 height_filtered_msg->header.frame_id = "imu"; // the frame seems arbitrary here 00324 00325 height_filtered_msg->height = imu_calcdata_msg->height * asctec::ASC_TO_ROS_HEIGHT; 00326 height_filtered_msg->climb = imu_calcdata_msg->dheight * asctec::ASC_TO_ROS_HEIGHT; 00327 } 00328 00329 void AsctecProc::createImuMsg(const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg, 00330 sensor_msgs::ImuPtr& imu_msg) 00331 { 00332 // set header info 00333 imu_msg->header.stamp = imu_calcdata_msg->header.stamp; 00334 imu_msg->header.frame_id = "imu"; 00335 00336 // copy over linear acceleration 00337 imu_msg->linear_acceleration.x = imu_calcdata_msg->acc_x_calib * asctec::ASC_TO_ROS_ACC * -1.0; 00338 imu_msg->linear_acceleration.y = imu_calcdata_msg->acc_y_calib * asctec::ASC_TO_ROS_ACC * -1.0; 00339 imu_msg->linear_acceleration.z = imu_calcdata_msg->acc_z_calib * asctec::ASC_TO_ROS_ACC * -1.0; 00340 00341 /* // Uncomment these if you use covariances 00342 // define linear acceleration variance 00343 imuMsg->linear_acceleration_covariance[0] = 1.0; 00344 imuMsg->linear_acceleration_covariance[1] = 0.0; 00345 imuMsg->linear_acceleration_covariance[2] = 0.0; 00346 imuMsg->linear_acceleration_covariance[3] = 0.0; 00347 imuMsg->linear_acceleration_covariance[4] = 1.0; 00348 imuMsg->linear_acceleration_covariance[5] = 0.0; 00349 imuMsg->linear_acceleration_covariance[6] = 0.0; 00350 imuMsg->linear_acceleration_covariance[7] = 0.0; 00351 imuMsg->linear_acceleration_covariance[8] = 1.0; 00352 */ 00353 // copy over angular_velocity - minus signs convert to ENU frame 00354 imu_msg->angular_velocity.x = imu_calcdata_msg->angvel_roll * asctec::ASC_TO_ROS_ANGVEL * -1.0; 00355 imu_msg->angular_velocity.y = imu_calcdata_msg->angvel_nick * asctec::ASC_TO_ROS_ANGVEL; 00356 imu_msg->angular_velocity.z = imu_calcdata_msg->angvel_yaw * asctec::ASC_TO_ROS_ANGVEL * -1.0; 00357 00358 /* // Uncomment these if you use covariances 00359 // define angular_velocity variance 00360 imuMsg->angular_velocity_covariance[0] = 1.0; 00361 imuMsg->angular_velocity_covariance[1] = 0.0; 00362 imuMsg->angular_velocity_covariance[2] = 0.0; 00363 imuMsg->angular_velocity_covariance[3] = 0.0; 00364 imuMsg->angular_velocity_covariance[4] = 1.0; 00365 imuMsg->angular_velocity_covariance[5] = 0.0; 00366 imuMsg->angular_velocity_covariance[6] = 0.0; 00367 imuMsg->angular_velocity_covariance[7] = 0.0; 00368 imuMsg->angular_velocity_covariance[8] = 1.0; 00369 */ 00370 00371 // calculate quaternion orientation - minus signs convert to ENU frame 00372 tf::Quaternion orientation; 00373 orientation.setRPY(imu_calcdata_msg->angle_roll * asctec::ASC_TO_ROS_ANGLE * -1.0, 00374 imu_calcdata_msg->angle_nick * asctec::ASC_TO_ROS_ANGLE, 00375 imu_calcdata_msg->angle_yaw * asctec::ASC_TO_ROS_ANGLE * -1.0); 00376 00377 imu_msg->orientation.x = orientation.getX(); 00378 imu_msg->orientation.y = orientation.getY(); 00379 imu_msg->orientation.z = orientation.getZ(); 00380 imu_msg->orientation.w = orientation.getW(); 00381 } 00382 00383 void AsctecProc::startMotors() 00384 { 00385 // set the stick to lower left, wait for motors to engage, 00386 // and reset stick 00387 00388 ROS_INFO ("Starting motors..."); 00389 00390 ctrl_input_publisher_.publish(ctrl_input_toggle_msg_); 00391 00392 for (int i = 0; i < 15; ++i) 00393 { 00394 if (motors_on_) break; 00395 //printf("\tt\n"); 00396 ros::Duration(0.20).sleep(); 00397 ctrl_input_publisher_.publish(ctrl_input_toggle_msg_); 00398 } 00399 00400 ctrl_input_publisher_.publish(ctrl_input_zero_msg_); 00401 00402 ROS_INFO("Motors are ON"); 00403 } 00404 00405 void AsctecProc::stopMotors() 00406 { 00407 // set the stick to lower left, wait for motors to disengage, 00408 // and reset stick 00409 00410 ROS_INFO ("Stopping motors..."); 00411 00412 ctrl_input_publisher_.publish(ctrl_input_toggle_msg_); 00413 00414 for (int i = 0; i < 15; ++i) 00415 { 00416 if (!motors_on_) break; 00417 //printf("\tt\n"); 00418 ros::Duration(0.20).sleep(); 00419 ctrl_input_publisher_.publish(ctrl_input_toggle_msg_); 00420 } 00421 00422 ctrl_input_publisher_.publish(ctrl_input_zero_msg_); 00423 00424 ROS_INFO("Motors are OFF"); 00425 } 00426 00427 void AsctecProc::assembleCtrlCommands() 00428 { 00429 // **** Assemble toggle-motors message 00430 00431 ctrl_input_toggle_msg_ = boost::make_shared<asctec_msgs::CtrlInput>(); 00432 00433 ctrl_input_toggle_msg_->thrust = 0; 00434 ctrl_input_toggle_msg_->roll = 0; 00435 ctrl_input_toggle_msg_->pitch = 0; 00436 ctrl_input_toggle_msg_->yaw = -2047; 00437 ctrl_input_toggle_msg_->ctrl = int(0b1100); 00438 00439 ctrl_input_toggle_msg_->chksum = ctrl_input_toggle_msg_->roll + ctrl_input_toggle_msg_->pitch + 00440 ctrl_input_toggle_msg_->yaw + ctrl_input_toggle_msg_->thrust + 00441 ctrl_input_toggle_msg_->ctrl - 21846; 00442 00443 // **** Assemble zero message 00444 00445 ctrl_input_zero_msg_ = boost::make_shared<asctec_msgs::CtrlInput>(); 00446 00447 ctrl_input_zero_msg_->thrust = 0; 00448 ctrl_input_zero_msg_->roll = 0; 00449 ctrl_input_zero_msg_->pitch = 0; 00450 ctrl_input_zero_msg_->yaw = 0; 00451 ctrl_input_zero_msg_->ctrl = int(0b1100); 00452 00453 ctrl_input_zero_msg_->chksum = ctrl_input_zero_msg_->roll + ctrl_input_zero_msg_->pitch + 00454 ctrl_input_zero_msg_->yaw + ctrl_input_zero_msg_->thrust + 00455 ctrl_input_zero_msg_->ctrl - 21846; 00456 } 00457 00458 void AsctecProc::publishCtrlInputMsg() 00459 { 00460 ROS_DEBUG("Publishing ctrl_input_msg"); 00461 00462 // **** Assemble the generic control input message 00463 00464 asctec_msgs::CtrlInputPtr ctrl_input_msg; 00465 ctrl_input_msg = boost::make_shared<asctec_msgs::CtrlInput>(); 00466 00467 ctrl_input_msg->thrust = ctrl_thrust_; 00468 ctrl_input_msg->roll = ctrl_roll_; 00469 ctrl_input_msg->pitch = ctrl_pitch_; 00470 ctrl_input_msg->yaw = ctrl_yaw_; 00471 ctrl_input_msg->ctrl = int(0b0000); 00472 00473 if (enable_ctrl_thrust_) ctrl_input_msg->ctrl |= 0b1000; // These are from CtrlInput.msg 00474 if (enable_ctrl_yaw_) ctrl_input_msg->ctrl |= 0b0100; 00475 if (enable_ctrl_roll_) ctrl_input_msg->ctrl |= 0b0010; 00476 if (enable_ctrl_pitch_) ctrl_input_msg->ctrl |= 0b0001; 00477 00478 // update checksum and timestamp, and publish 00479 ctrl_input_msg->chksum = ctrl_input_msg->roll + ctrl_input_msg->pitch + 00480 ctrl_input_msg->yaw + ctrl_input_msg->thrust + 00481 ctrl_input_msg->ctrl - 21846; 00482 ctrl_input_msg->header.stamp = ros::Time::now(); 00483 ctrl_input_publisher_.publish(ctrl_input_msg); 00484 } 00485 00486 } // end namespace asctec