$search
00001 //================================================================================================= 00002 // Copyright (c) 2012, Johannes Meyer and Martin Nowara, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #include <hector_pose_estimation/hector_pose_estimation_rtt.h> 00030 #include "services.h" 00031 00032 #include <hector_pose_estimation/system/generic_quaternion_system_model.h> 00033 #include <hector_pose_estimation/measurements/poseupdate.h> 00034 #include <hector_pose_estimation/measurements/height.h> 00035 #include <hector_pose_estimation/measurements/magnetic.h> 00036 #include <hector_pose_estimation/measurements/gps.h> 00037 00038 #include <rtt/Component.hpp> 00039 00040 namespace hector_pose_estimation { 00041 00042 PoseEstimationTaskContext::PoseEstimationTaskContext(const std::string& name, const SystemPtr& system) 00043 : RTT::TaskContext(name, RTT::TaskContext::PreOperational) 00044 , PoseEstimation(system) 00045 { 00046 if (!system) setSystemModel(new GenericQuaternionSystemModel); 00047 00048 this->addEventPort("raw_imu", imu_input_); 00049 this->addPort("poseupdate", pose_update_input_); 00050 this->addPort("magnetic", magnetic_input_); 00051 this->addPort("pressure_height", height_input_); 00052 this->addPort("fix", gps_input_); 00053 this->addPort("fix_velocity", gps_velocity_input_); 00054 this->addPort("state", state_output_); 00055 this->addPort("imu", imu_output_); 00056 this->addPort("pose", pose_output_); 00057 this->addPort("velocity", velocity_output_); 00058 this->addPort("global", global_position_output_); 00059 this->addPort("angular_velocity_bias", angular_velocity_bias_output_); 00060 this->addPort("linear_acceleration_bias", linear_acceleration_bias_output_); 00061 this->addEventPort("syscommand", command_input_, boost::bind(&PoseEstimationTaskContext::commandCallback, this, _1)); 00062 00063 param_namespace_ = "~"; 00064 this->addProperty("param_namespace", param_namespace_); 00065 00066 this->addOperation("reset", &PoseEstimationTaskContext::reset, this, RTT::OwnThread); 00067 this->addOperation("getSystemStatus", &PoseEstimation::getSystemStatus, static_cast<PoseEstimation *>(this), RTT::OwnThread); 00068 this->addOperation("getMeasurementStatus", &PoseEstimation::getMeasurementStatus, static_cast<PoseEstimation *>(this), RTT::OwnThread); 00069 00070 PoseEstimation::addMeasurement(new PoseUpdate("PoseUpdate")); 00071 PoseEstimation::addMeasurement(new Height("Height")); 00072 PoseEstimation::addMeasurement(new Magnetic("Magnetic")); 00073 PoseEstimation::addMeasurement(new GPS("GPS")); 00074 00075 this->provides()->addService(RTT::Service::shared_ptr(new SystemService(this, getSystem(), "System"))); 00076 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it) { 00077 this->provides()->addService(RTT::Service::shared_ptr(new MeasurementService(this, *it))); 00078 } 00079 00080 PoseEstimation::getParameters().registerParamsRos(ros::NodeHandle(param_namespace_)); 00081 PoseEstimation::parameters().registerParams(boost::bind(®isterParamAsProperty, _1, this->properties())); 00082 } 00083 00084 PoseEstimationTaskContext::~PoseEstimationTaskContext() 00085 { 00086 stop(); 00087 cleanup(); 00088 } 00089 00090 bool PoseEstimationTaskContext::configureHook() 00091 { 00092 if (!PoseEstimation::init()) { 00093 RTT::log(RTT::Error) << "Intitialization of pose estimation failed!" << RTT::endlog(); 00094 return false; 00095 } 00096 00097 // publish initial state 00098 updateOutputs(); 00099 00100 return true; 00101 } 00102 00103 void PoseEstimationTaskContext::cleanupHook() 00104 { 00105 PoseEstimation::cleanup(); 00106 } 00107 00108 bool PoseEstimationTaskContext::startHook() 00109 { 00110 return true; 00111 } 00112 00113 void PoseEstimationTaskContext::stopHook() 00114 { 00115 } 00116 00117 void PoseEstimationTaskContext::updateHook() 00118 { 00119 while(magnetic_input_.read(magnetic_) == RTT::NewData && PoseEstimation::getMeasurement("Magnetic")) 00120 { 00121 Magnetic::MeasurementVector update(3); 00122 update(1) = magnetic_.vector.x; 00123 update(2) = magnetic_.vector.y; 00124 update(3) = magnetic_.vector.z; 00125 PoseEstimation::getMeasurement("Magnetic")->add(Magnetic::Update(update)); 00126 } 00127 00128 while(height_input_.read(height_) == RTT::NewData && PoseEstimation::getMeasurement("Height")) 00129 { 00130 Height::Update update; 00131 #ifdef USE_MAV_MSGS 00132 update = height_.height; 00133 #else 00134 update = height_.point.z; 00135 #endif 00136 PoseEstimation::getMeasurement("Height")->add(update); 00137 } 00138 00139 while(gps_input_.read(gps_) == RTT::NewData && gps_velocity_input_.read(gps_velocity_) != RTT::NoData && PoseEstimation::getMeasurement("GPS")) 00140 { 00141 if (gps_.status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX) 00142 { 00143 GPS::Update update; 00144 update.latitude = gps_.latitude * M_PI/180.0; 00145 update.longitude = gps_.longitude * M_PI/180.0; 00146 update.velocity_north = gps_velocity_.vector.x; 00147 update.velocity_east = -gps_velocity_.vector.y; 00148 PoseEstimation::getMeasurement("GPS")->add(update); 00149 } 00150 } 00151 00152 while(pose_update_input_.read(pose_update_) == RTT::NewData && PoseEstimation::getMeasurement("PoseUpdate")) 00153 { 00154 PoseEstimation::getMeasurement("PoseUpdate")->add(PoseUpdate::Update(pose_update_)); 00155 } 00156 00157 while(imu_input_.read(imu_in_) == RTT::NewData) { 00158 PoseEstimation::update(ImuInput(imu_in_), imu_in_.header.stamp); 00159 updateOutputs(); 00160 } 00161 } 00162 00163 void PoseEstimationTaskContext::updateOutputs() 00164 { 00165 getState(state_); 00166 state_output_.write(state_); 00167 00168 if (imu_output_.connected()) { 00169 imu_out_.header = state_.header; 00170 imu_out_.orientation = state_.pose.pose.orientation; 00171 getImuWithBiases(imu_out_.linear_acceleration, imu_out_.angular_velocity); 00172 imu_output_.write(imu_out_); 00173 } 00174 00175 if (pose_output_.connected()) { 00176 pose_.header = state_.header; 00177 pose_.pose = state_.pose.pose; 00178 pose_output_.write(pose_); 00179 } 00180 00181 if (velocity_output_.connected()) { 00182 velocity_.header = state_.header; 00183 velocity_.vector = state_.twist.twist.linear; 00184 velocity_output_.write(velocity_); 00185 } 00186 00187 if (global_position_output_.connected()) { 00188 global_position_.header = state_.header; 00189 getGlobalPosition(global_position_.latitude, global_position_.longitude, global_position_.altitude); 00190 global_position_.latitude *= 180.0/M_PI; 00191 global_position_.longitude *= 180.0/M_PI; 00192 if (getSystemStatus() & STATE_XY_POSITION) { 00193 global_position_.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; 00194 } else { 00195 global_position_.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; 00196 } 00197 global_position_output_.write(global_position_); 00198 } 00199 00200 if (angular_velocity_bias_output_.connected() || linear_acceleration_bias_output_.connected()) { 00201 getHeader(angular_velocity_bias_.header); 00202 getHeader(linear_acceleration_bias_.header); 00203 getBias(angular_velocity_bias_, linear_acceleration_bias_); 00204 angular_velocity_bias_output_.write(angular_velocity_bias_); 00205 linear_acceleration_bias_output_.write(linear_acceleration_bias_); 00206 } 00207 } 00208 00209 00210 void PoseEstimationTaskContext::reset() { 00211 RTT::log(RTT::Info) << "Resetting pose_estimation" << RTT::endlog(); 00212 PoseEstimation::reset(); 00213 updateOutputs(); 00214 } 00215 00216 void PoseEstimationTaskContext::commandCallback(RTT::base::PortInterface *) { 00217 std_msgs::String command; 00218 if (command_input_.read(command) == RTT::NewData) { 00219 if (command.data == "reset") reset(); 00220 } 00221 } 00222 00223 } // namespace 00224 00225 ORO_CREATE_COMPONENT( hector_pose_estimation::PoseEstimationTaskContext )