Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_pose_estimation/rtt/taskcontext.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 <hector_pose_estimation/ros/parameters.h>
00039
00040 #include <rtt/Component.hpp>
00041
00042 namespace hector_pose_estimation {
00043
00044 PoseEstimationTaskContext::PoseEstimationTaskContext(const std::string& name, const SystemPtr& system)
00045 : RTT::TaskContext(name, RTT::TaskContext::PreOperational)
00046 , PoseEstimation(system)
00047 {
00048 if (!system) setSystemModel(new GenericQuaternionSystemModel);
00049
00050 this->addEventPort("raw_imu", imu_input_);
00051 this->addPort("poseupdate", pose_update_input_);
00052 this->addPort("magnetic", magnetic_input_);
00053 this->addPort("pressure_height", height_input_);
00054 this->addPort("fix", gps_input_);
00055 this->addPort("fix_velocity", gps_velocity_input_);
00056 this->addPort("state", state_output_);
00057 this->addPort("imu", imu_output_);
00058 this->addPort("pose", pose_output_);
00059 this->addPort("velocity", velocity_output_);
00060 this->addPort("global", global_position_output_);
00061 this->addPort("angular_velocity_bias", angular_velocity_bias_output_);
00062 this->addPort("linear_acceleration_bias", linear_acceleration_bias_output_);
00063 this->addEventPort("syscommand", command_input_, boost::bind(&PoseEstimationTaskContext::commandCallback, this, _1));
00064
00065 param_namespace_ = "~";
00066 this->addProperty("param_namespace", param_namespace_);
00067
00068 this->addOperation("reset", &PoseEstimationTaskContext::reset, this, RTT::OwnThread);
00069 this->addOperation("getSystemStatus", &PoseEstimation::getSystemStatus, static_cast<PoseEstimation *>(this), RTT::OwnThread);
00070 this->addOperation("getMeasurementStatus", &PoseEstimation::getMeasurementStatus, static_cast<PoseEstimation *>(this), RTT::OwnThread);
00071
00072 PoseEstimation::addMeasurement(new PoseUpdate("PoseUpdate"));
00073 PoseEstimation::addMeasurement(new Height("Height"));
00074 PoseEstimation::addMeasurement(new Magnetic("Magnetic"));
00075 PoseEstimation::addMeasurement(new GPS("GPS"));
00076
00077 this->provides()->addService(RTT::Service::shared_ptr(new SystemService(this, getSystem(), "System")));
00078 for(Measurements::iterator it = measurements_.begin(); it != measurements_.end(); ++it) {
00079 this->provides()->addService(RTT::Service::shared_ptr(new MeasurementService(this, *it)));
00080 }
00081
00082 PoseEstimation::getParameters().initialize(ParameterRegistryROS(ros::NodeHandle(param_namespace_)));
00083 PoseEstimation::parameters().initialize(boost::bind(®isterParamAsProperty, _1, this->properties()));
00084 }
00085
00086 PoseEstimationTaskContext::~PoseEstimationTaskContext()
00087 {
00088 stop();
00089 cleanup();
00090 }
00091
00092 bool PoseEstimationTaskContext::configureHook()
00093 {
00094 if (!PoseEstimation::init()) {
00095 RTT::log(RTT::Error) << "Intitialization of pose estimation failed!" << RTT::endlog();
00096 return false;
00097 }
00098
00099
00100 updateOutputs();
00101
00102 return true;
00103 }
00104
00105 void PoseEstimationTaskContext::cleanupHook()
00106 {
00107 PoseEstimation::cleanup();
00108 }
00109
00110 bool PoseEstimationTaskContext::startHook()
00111 {
00112 return true;
00113 }
00114
00115 void PoseEstimationTaskContext::stopHook()
00116 {
00117 }
00118
00119 void PoseEstimationTaskContext::updateHook()
00120 {
00121 while(magnetic_input_.read(magnetic_) == RTT::NewData && PoseEstimation::getMeasurement("Magnetic"))
00122 {
00123 Magnetic::MeasurementVector update(3);
00124 update(1) = magnetic_.vector.x;
00125 update(2) = magnetic_.vector.y;
00126 update(3) = magnetic_.vector.z;
00127 PoseEstimation::getMeasurement("Magnetic")->add(Magnetic::Update(update));
00128 }
00129
00130 while(height_input_.read(height_) == RTT::NewData && PoseEstimation::getMeasurement("Height"))
00131 {
00132 Height::Update update;
00133 #ifdef USE_MAV_MSGS
00134 update = height_.height;
00135 #else
00136 update = height_.point.z;
00137 #endif
00138 PoseEstimation::getMeasurement("Height")->add(update);
00139 }
00140
00141 while(gps_input_.read(gps_) == RTT::NewData && gps_velocity_input_.read(gps_velocity_) != RTT::NoData && PoseEstimation::getMeasurement("GPS"))
00142 {
00143 if (gps_.status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX)
00144 {
00145 GPS::Update update;
00146 update.latitude = gps_.latitude * M_PI/180.0;
00147 update.longitude = gps_.longitude * M_PI/180.0;
00148 update.velocity_north = gps_velocity_.vector.x;
00149 update.velocity_east = -gps_velocity_.vector.y;
00150 PoseEstimation::getMeasurement("GPS")->add(update);
00151 }
00152 }
00153
00154 while(pose_update_input_.read(pose_update_) == RTT::NewData && PoseEstimation::getMeasurement("PoseUpdate"))
00155 {
00156 PoseEstimation::getMeasurement("PoseUpdate")->add(PoseUpdate::Update(pose_update_));
00157 }
00158
00159 while(imu_input_.read(imu_in_) == RTT::NewData) {
00160 PoseEstimation::update(ImuInput(imu_in_), imu_in_.header.stamp);
00161 updateOutputs();
00162 }
00163 }
00164
00165 void PoseEstimationTaskContext::updateOutputs()
00166 {
00167 getState(state_);
00168 state_output_.write(state_);
00169
00170 if (imu_output_.connected()) {
00171 imu_out_.header = state_.header;
00172 imu_out_.orientation = state_.pose.pose.orientation;
00173 getImuWithBiases(imu_out_.linear_acceleration, imu_out_.angular_velocity);
00174 imu_output_.write(imu_out_);
00175 }
00176
00177 if (pose_output_.connected()) {
00178 pose_.header = state_.header;
00179 pose_.pose = state_.pose.pose;
00180 pose_output_.write(pose_);
00181 }
00182
00183 if (velocity_output_.connected()) {
00184 velocity_.header = state_.header;
00185 velocity_.vector = state_.twist.twist.linear;
00186 velocity_output_.write(velocity_);
00187 }
00188
00189 if (global_position_output_.connected()) {
00190 global_position_.header = state_.header;
00191 getGlobalPosition(global_position_.latitude, global_position_.longitude, global_position_.altitude);
00192 global_position_.latitude *= 180.0/M_PI;
00193 global_position_.longitude *= 180.0/M_PI;
00194 if (getSystemStatus() & STATE_XY_POSITION) {
00195 global_position_.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
00196 } else {
00197 global_position_.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00198 }
00199 global_position_output_.write(global_position_);
00200 }
00201
00202 if (angular_velocity_bias_output_.connected() || linear_acceleration_bias_output_.connected()) {
00203 getHeader(angular_velocity_bias_.header);
00204 getHeader(linear_acceleration_bias_.header);
00205 getBias(angular_velocity_bias_, linear_acceleration_bias_);
00206 angular_velocity_bias_output_.write(angular_velocity_bias_);
00207 linear_acceleration_bias_output_.write(linear_acceleration_bias_);
00208 }
00209 }
00210
00211
00212 void PoseEstimationTaskContext::reset() {
00213 RTT::log(RTT::Info) << "Resetting pose_estimation" << RTT::endlog();
00214 PoseEstimation::reset();
00215 updateOutputs();
00216 }
00217
00218 void PoseEstimationTaskContext::commandCallback(RTT::base::PortInterface *) {
00219 std_msgs::String command;
00220 if (command_input_.read(command) == RTT::NewData) {
00221 if (command.data == "reset") reset();
00222 }
00223 }
00224
00225 }
00226
00227 ORO_CREATE_COMPONENT( hector_pose_estimation::PoseEstimationTaskContext )