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/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
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 }
00224
00225 ORO_CREATE_COMPONENT( hector_pose_estimation::PoseEstimationTaskContext )