ethzasl-msf - Modular Sensor Fusion
Time delay compensated single and multi sensor fusion framework based on an EKF
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
msf_sensormanagerROS.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
3  * You can contact the author at <slynen at ethz dot ch>
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at
8  *
9  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  */
17 #ifndef SENSORMANAGERROS_H
18 #define SENSORMANAGERROS_H
19 
20 #include <ros/ros.h>
21 #include <dynamic_reconfigure/server.h>
22 
23 // Message includes.
24 #include <sensor_fusion_comm/DoubleArrayStamped.h>
25 #include <sensor_fusion_comm/ExtState.h>
26 #include <sensor_fusion_comm/ExtEkf.h>
27 #include <geometry_msgs/PoseWithCovarianceStamped.h>
28 #include <sensor_msgs/Imu.h>
29 #include <asctec_hl_comm/mav_imu.h>
30 #include <tf/transform_broadcaster.h>
31 
32 #include <msf_core/MSF_CoreConfig.h>
33 #include <msf_core/msf_sensormanager.h>
34 #include <msf_core/msf_types.hpp>
35 
36 namespace msf_core {
37 
38 enum {
40  // Here: p, v, q, bw, bw = 16
41  HLI_EKF_STATE_SIZE = 16
42 };
43 
44 typedef dynamic_reconfigure::Server<msf_core::MSF_CoreConfig> ReconfigureServer;
45 
50 template<typename EKFState_T>
51 struct MSF_SensorManagerROS : public msf_core::MSF_SensorManager<EKFState_T> {
52  protected:
53  msf_core::MSF_CoreConfig config_;
54  private:
55 
56  typedef typename EKFState_T::StateDefinition_T StateDefinition_T;
57  typedef typename EKFState_T::StateSequence_T StateSequence_T;
58 
59  ReconfigureServer *reconfServer_;
60 
61  ros::Publisher pubState_;
62  ros::Publisher pubPose_;
63  ros::Publisher pubPoseAfterUpdate_;
64  ros::Publisher pubPoseCrtl_;
65  ros::Publisher pubCorrect_;
66 
67  mutable tf::TransformBroadcaster tf_broadcaster_;
68 
69  sensor_fusion_comm::ExtEkf hl_state_buf_;
70 
71  public:
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
74  MSF_SensorManagerROS(ros::NodeHandle pnh = ros::NodeHandle("~core")) {
75  reconfServer_ = new ReconfigureServer(pnh);
76  ReconfigureServer::CallbackType f = boost::bind(
77  &MSF_SensorManagerROS::Config, this, _1, _2);
78  reconfServer_->setCallback(f);
79 
80  pnh.param("data_playback", this->data_playback_, false);
81 
82  ros::NodeHandle nh("msf_core");
83 
84  pubState_ = nh.advertise < sensor_fusion_comm::DoubleArrayStamped
85  > ("state_out", 100);
86  pubCorrect_ = nh.advertise < sensor_fusion_comm::ExtEkf > ("correction", 1);
87  pubPose_ = nh.advertise < geometry_msgs::PoseWithCovarianceStamped
88  > ("pose", 100);
89  pubPoseAfterUpdate_ = nh.advertise
90  < geometry_msgs::PoseWithCovarianceStamped > ("pose_after_update", 100);
91  pubPoseCrtl_ = nh.advertise < sensor_fusion_comm::ExtState
92  > ("ext_state", 1);
93 
94  hl_state_buf_.state.resize(HLI_EKF_STATE_SIZE, 0);
95 
96  // Print published/subscribed topics.
97  ros::V_string topics;
98  ros::this_node::getSubscribedTopics(topics);
99  std::string nodeName = ros::this_node::getName();
100  std::string topicsStr = nodeName + ":\n\tsubscribed to topics:\n";
101  for (unsigned int i = 0; i < topics.size(); i++)
102  topicsStr += ("\t\t" + topics.at(i) + "\n");
103 
104  topicsStr += "\tadvertised topics:\n";
105  ros::this_node::getAdvertisedTopics(topics);
106  for (unsigned int i = 0; i < topics.size(); i++)
107  topicsStr += ("\t\t" + topics.at(i) + "\n");
108 
109  MSF_INFO_STREAM(""<< topicsStr);
110  }
111 
112  virtual ~MSF_SensorManagerROS() {
113  delete reconfServer_;
114  }
115 
119  virtual void coreConfig(msf_core::MSF_CoreConfig &config, uint32_t level) {
120  UNUSED(config);
121  UNUSED(level);
122  }
123 
128  void Config(msf_core::MSF_CoreConfig &config, uint32_t level) {
129  config_ = config;
130  coreConfig(config, level);
131  }
132 
133  // Set the latest HL state which is needed to compute the correction to be
134  // send back to the HL.
135  void setHLStateBuffer(const sensor_fusion_comm::ExtEkf& msg) {
136  hl_state_buf_ = msg;
137  }
138 
139  // Parameter getters.
140  virtual bool getParam_fixed_bias() const {
141  return config_.core_fixed_bias;
142  }
143  virtual double getParam_noise_acc() const {
144  return config_.core_noise_acc;
145  }
146  virtual double getParam_noise_accbias() const {
147  return config_.core_noise_accbias;
148  }
149  virtual double getParam_noise_gyr() const {
150  return config_.core_noise_gyr;
151  }
152  virtual double getParam_noise_gyrbias() const {
153  return config_.core_noise_gyrbias;
154  }
155  virtual double getParam_fuzzythres() const {
156  return 0.1;
157  }
158  virtual void publishStateInitial(const shared_ptr<EKFState_T>& state) const {
163  sensor_fusion_comm::ExtEkf msgCorrect_;
164  msgCorrect_.state.resize(HLI_EKF_STATE_SIZE, 0);
165  msgCorrect_.header.stamp = ros::Time(state->time);
166  msgCorrect_.header.seq = 0;
167  msgCorrect_.angular_velocity.x = 0;
168  msgCorrect_.angular_velocity.y = 0;
169  msgCorrect_.angular_velocity.z = 0;
170  msgCorrect_.linear_acceleration.x = 0;
171  msgCorrect_.linear_acceleration.y = 0;
172  msgCorrect_.linear_acceleration.z = 0;
173 
174  msgCorrect_.state.resize(HLI_EKF_STATE_SIZE);
175  boost::fusion::for_each(
176  state->statevars,
177  msf_tmp::CoreStatetoDoubleArray<std::vector<float>, StateSequence_T>(
178  msgCorrect_.state));
179 
180  msgCorrect_.flag = sensor_fusion_comm::ExtEkf::initialization;
181  pubCorrect_.publish(msgCorrect_);
182 
183  }
184 
185  virtual void publishStateAfterPropagation(
186  const shared_ptr<EKFState_T>& state) const {
187 
188  if (pubPoseCrtl_.getNumSubscribers()) {
189  static int msg_seq = 0;
190 
191  geometry_msgs::PoseWithCovarianceStamped msgPose;
192  msgPose.header.stamp = ros::Time(state->time);
193  msgPose.header.seq = msg_seq++;
194  msgPose.header.frame_id = "/world";
195  state->toPoseMsg(msgPose);
196  pubPose_.publish(msgPose);
197 
198  sensor_fusion_comm::ExtState msgPoseCtrl;
199  msgPoseCtrl.header = msgPose.header;
200  state->toExtStateMsg(msgPoseCtrl);
201  pubPoseCrtl_.publish(msgPoseCtrl);
202 
203  }
204  }
205 
206  virtual void publishStateAfterUpdate(
207  const shared_ptr<EKFState_T>& state) const {
208 
209  static int msg_seq = 0;
210 
211  sensor_fusion_comm::ExtEkf msgCorrect_;
212  msgCorrect_.state.resize(HLI_EKF_STATE_SIZE);
213  msgCorrect_.header.stamp = ros::Time(state->time);
214  msgCorrect_.header.seq = msg_seq;
215  msgCorrect_.angular_velocity.x = 0;
216  msgCorrect_.angular_velocity.y = 0;
217  msgCorrect_.angular_velocity.z = 0;
218  msgCorrect_.linear_acceleration.x = 0;
219  msgCorrect_.linear_acceleration.y = 0;
220  msgCorrect_.linear_acceleration.z = 0;
221 
222  // Prevent junk being sent to the external state propagation when data
223  // playback is (accidentally) on.
224  if (pubCorrect_.getNumSubscribers() > 0) {
225  if (this->data_playback_) {
226  for (int i = 0; i < HLI_EKF_STATE_SIZE; ++i) {
227  msgCorrect_.state[i] = 0;
228  }
229  msgCorrect_.state[6] = 1; //w
230  msgCorrect_.flag = sensor_fusion_comm::ExtEkf::initialization;
231 
232  MSF_ERROR_STREAM_THROTTLE(
233  1, __FUNCTION__ << " You have connected the external propagation "
234  "topic but at the same time data_playback is on.");
235 
236  } else {
237  const EKFState_T& state_const = *state;
238  msgCorrect_.state[0] =
239  state_const.template get<StateDefinition_T::p>()[0]
240  - hl_state_buf_.state[0];
241  msgCorrect_.state[1] =
242  state_const.template get<StateDefinition_T::p>()[1]
243  - hl_state_buf_.state[1];
244  msgCorrect_.state[2] =
245  state_const.template get<StateDefinition_T::p>()[2]
246  - hl_state_buf_.state[2];
247  msgCorrect_.state[3] =
248  state_const.template get<StateDefinition_T::v>()[0]
249  - hl_state_buf_.state[3];
250  msgCorrect_.state[4] =
251  state_const.template get<StateDefinition_T::v>()[1]
252  - hl_state_buf_.state[4];
253  msgCorrect_.state[5] =
254  state_const.template get<StateDefinition_T::v>()[2]
255  - hl_state_buf_.state[5];
256 
257  Eigen::Quaterniond hl_q(hl_state_buf_.state[6], hl_state_buf_.state[7],
258  hl_state_buf_.state[8], hl_state_buf_.state[9]);
259  Eigen::Quaterniond qbuff_q = hl_q.inverse()
260  * state_const.template get<StateDefinition_T::q>();
261  msgCorrect_.state[6] = qbuff_q.w();
262  msgCorrect_.state[7] = qbuff_q.x();
263  msgCorrect_.state[8] = qbuff_q.y();
264  msgCorrect_.state[9] = qbuff_q.z();
265 
266  msgCorrect_.state[10] =
267  state_const.template get<StateDefinition_T::b_w>()[0]
268  - hl_state_buf_.state[10];
269  msgCorrect_.state[11] =
270  state_const.template get<StateDefinition_T::b_w>()[1]
271  - hl_state_buf_.state[11];
272  msgCorrect_.state[12] =
273  state_const.template get<StateDefinition_T::b_w>()[2]
274  - hl_state_buf_.state[12];
275  msgCorrect_.state[13] =
276  state_const.template get<StateDefinition_T::b_a>()[0]
277  - hl_state_buf_.state[13];
278  msgCorrect_.state[14] =
279  state_const.template get<StateDefinition_T::b_a>()[1]
280  - hl_state_buf_.state[14];
281  msgCorrect_.state[15] =
282  state_const.template get<StateDefinition_T::b_a>()[2]
283  - hl_state_buf_.state[15];
284 
285  msgCorrect_.flag = sensor_fusion_comm::ExtEkf::state_correction;
286  }
287 
288  if (state->checkStateForNumeric()) { // If not NaN.
289  pubCorrect_.publish(msgCorrect_);
290  } else {
291  MSF_WARN_STREAM_THROTTLE(
292  1, "Not sending updates to external EKF, because state NaN/inf.");
293  }
294  }
295 
296  // Publish state.
297  sensor_fusion_comm::DoubleArrayStamped msgState;
298  msgState.header = msgCorrect_.header;
299  state->toFullStateMsg(msgState);
300  pubState_.publish(msgState);
301 
302  if (pubPoseAfterUpdate_.getNumSubscribers()) {
303 
304  // Publish pose after correction with covariance.
305  geometry_msgs::PoseWithCovarianceStamped msgPose;
306  msgPose.header.stamp = ros::Time(state->time);
307  msgPose.header.seq = msg_seq;
308  msgPose.header.frame_id = "/world";
309 
310  state->toPoseMsg(msgPose);
311  pubPoseAfterUpdate_.publish(msgPose);
312  }
313 
314  {
315  tf::Transform transform;
316  const EKFState_T& state_const = *state;
317  const Eigen::Matrix<double, 3, 1>& pos = state_const
318  .template get<StateDefinition_T::p>();
319  const Eigen::Quaterniond& ori = state_const
320  .template get<StateDefinition_T::q>();
321  transform.setOrigin(tf::Vector3(pos[0], pos[1], pos[2]));
322  transform.setRotation(tf::Quaternion(ori.x(), ori.y(), ori.z(), ori.w()));
323  tf_broadcaster_.sendTransform(
324  tf::StampedTransform(
325  transform, ros::Time::now() /*ros::Time(latestState->time_)*/,
326  "world", "state"));
327  }
328  msg_seq++;
329  }
330 };
331 
332 }
333 #endif // SENSORMANAGERROS_H
Generated on Wed Nov 6 2013 22:21:53 for ethzasl-msf - Modular Sensor Fusion by   doxygen 1.8.1.2