ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
funvehiclemotionstateconverter.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <math.h>
17 #include <tf/tf.h>
18 #include <tf/transform_broadcaster.h>
19 #include <tf/LinearMath/Quaternion.h>
20 #include <tf2/LinearMath/Matrix3x3.h>
21 #include "quaternion.h"
23 #include <adore/fun/afactory.h>
25 #include <adore_if_ros_msg/Border.h>
26 #include <adore_if_ros_msg/NavigationGoal.h>
27 #include <adore_if_ros_msg/SetPointRequest.h>
28 #include <adore_if_ros_msg/TerminalRequest.h>
29 #include <adore_if_ros_msg/WheelSpeed.h>
30 #include <std_msgs/Float64.h>
31 #include <std_msgs/Float32.h>
32 #include <std_msgs/Int8.h>
33 #include <std_msgs/Bool.h>
34 #include <nav_msgs/Odometry.h>
35 #include <adore/params/afactory.h>
36 #include <list>
37 
38 namespace adore
39 {
40  namespace if_ROS
41  {
46  class MotionStateReader:public adore::mad::AReader<adore::fun::VehicleMotionState9d>
47  {
48  private:
49  ros::Subscriber odom_subscriber_;
50  ros::Subscriber steering_subscriber_;
51  ros::Subscriber acceleration_subscriber_;
56  bool changed_;
59 
60  void receive_odom(nav_msgs::OdometryConstPtr msg)
61  {
62  data_.setTime(msg->header.stamp.toSec());
63  data_.setX(msg->pose.pose.position.x);
64  data_.setY(msg->pose.pose.position.y);
65  data_.setZ(msg->pose.pose.position.z);
67  data_.setPSI(qc.quaternionToHeading(msg->pose.pose));
68  data_.setvx(msg->twist.twist.linear.x);
69  data_.setvy(msg->twist.twist.linear.y);
70  data_.setOmega(msg->twist.twist.angular.z);
71  odom_initialized_ = true;
72  changed_ = true;
73  }
74  void receive_steering(std_msgs::Float32ConstPtr msg)
75  {
76  if(params_==nullptr)
77  {
79  if(pfactory==nullptr)
80  {
81  std::cerr << " WARNING not reading motion state (steering angle) until parameter factory and steeringRatio parameter are available."
82  << std::endl;
83  return;
84  }
85  else
86  {
87  params_ = pfactory->getVehicle();
88  }
89  }
90  data_.setDelta(msg->data / params_->get_steeringRatio());
91  steering_initialized_ = true;
92  changed_ = true;
93  }
94  void receive_acceleration(std_msgs::Float32ConstPtr msg)
95  {
96  data_.setAx(msg->data);
98  changed_ = true;
99  }
100 
101  public:
102  MotionStateReader(ros::NodeHandle* n,
103  const std::string odom_topic,
104  const std::string accelerationTopic,
105  const std::string steeringAngleTopic,
106  int qsize)
107  :odom_initialized_(false),
108  steering_initialized_(false),
110  changed_(false)
111  {
112  bool no_delay;
113  n->param("/tcp_no_delay", no_delay, false);
114  odom_subscriber_ = n->subscribe(odom_topic,qsize,&MotionStateReader::receive_odom,this,ros::TransportHints().tcpNoDelay(no_delay));
115  acceleration_subscriber_ = n->subscribe(accelerationTopic,qsize,&MotionStateReader::receive_acceleration,this,ros::TransportHints().tcpNoDelay(no_delay));
116  steering_subscriber_ = n->subscribe(steeringAngleTopic,qsize,&MotionStateReader::receive_steering,this,ros::TransportHints().tcpNoDelay(no_delay));
117  params_ = nullptr;
118  }
119  virtual bool hasData() const override
120  {
122  }
123  virtual bool hasUpdate() const override
124  {
125  return hasData() && changed_;
126  }
127  virtual void getData(adore::fun::VehicleMotionState9d& value) override
128  {
129  value = data_;
130  }
131 
132  };
133 
138  class MotionStateFeed:public adore::mad::AFeed<adore::fun::VehicleMotionState9d>
139  {
140  private:
141  ros::Subscriber odom_subscriber_;
142  ros::Subscriber steering_subscriber_;
143  ros::Subscriber acceleration_subscriber_;
144  double ax_;
145  double delta_;
147  std::list<adore::fun::VehicleMotionState9d> queue_;
148 
149  void receive_odom(nav_msgs::OdometryConstPtr msg)
150  {
152  data.setTime(msg->header.stamp.toSec());
153  data.setX(msg->pose.pose.position.x);
154  data.setY(msg->pose.pose.position.y);
155  data.setZ(msg->pose.pose.position.z);
157  data.setPSI(qc.quaternionToHeading(msg->pose.pose));
158  data.setvx(msg->twist.twist.linear.x);
159  data.setvy(msg->twist.twist.linear.y);
160  data.setOmega(msg->twist.twist.angular.z);
161  data.setAx(ax_);
162  data.setDelta(delta_);
163  queue_.push_back(data);
164  }
165  void receive_steering(std_msgs::Float32ConstPtr msg)
166  {
167  if(params_==nullptr)
168  {
170  if(pfactory==nullptr)
171  {
172  std::cerr << " WARNING not reading motion state (steering angle) until parameter factory and steeringRatio parameter are available."
173  << std::endl;
174  return;
175  }
176  else
177  {
178  params_ = pfactory->getVehicle();
179  }
180  }
181  delta_ = msg->data / params_->get_steeringRatio();
182  }
183  void receive_acceleration(std_msgs::Float32ConstPtr msg)
184  {
185  ax_ = msg->data;
186  }
187 
188  public:
189  MotionStateFeed(ros::NodeHandle* n,
190  const std::string odom_topic,
191  const std::string accelerationTopic,
192  const std::string steeringAngleTopic,
193  int qsize)
194  {
195  bool no_delay;
196  ax_ = 0.0;
197  delta_ = 0.0;
198  n->param("/tcp_no_delay", no_delay, false);
199  odom_subscriber_ = n->subscribe(odom_topic,qsize,&MotionStateFeed::receive_odom,this,ros::TransportHints().tcpNoDelay(no_delay));
200  acceleration_subscriber_ = n->subscribe(accelerationTopic,qsize,&MotionStateFeed::receive_acceleration,this,ros::TransportHints().tcpNoDelay(no_delay));
201  steering_subscriber_ = n->subscribe(steeringAngleTopic,qsize,&MotionStateFeed::receive_steering,this,ros::TransportHints().tcpNoDelay(no_delay));
202  params_ = nullptr;
203  }
207  virtual bool hasNext() const override
208  {
209  return queue_.size()>0;
210  }
214  virtual void getNext(adore::fun::VehicleMotionState9d& value)override
215  {
216  value = *queue_.begin();
217  queue_.pop_front();
218  }
222  virtual void getLatest(adore::fun::VehicleMotionState9d& value)override
223  {
224  value = *queue_.rbegin();
225  queue_.clear();
226  }
227 
228  };
229 
230 
234  class VehicleExtendedStateReader:public adore::mad::AReader<adore::fun::VehicleExtendedState>
235  {
236  private:
237  ros::Subscriber gear_subscriber_;
238  ros::Subscriber steering_subscriber_;
239  ros::Subscriber acceleration_subscriber_;
241  ros::Subscriber leftIndicator_subscriber_;
242  ros::Subscriber rightIndicator_subscriber_;
251  bool changed_;
253 
254  void receive_gear(std_msgs::Int8ConstPtr msg)
255  {
257  gear_initialized_ = true;
258  changed_ = true;
259  }
260  void receive_steering(std_msgs::BoolConstPtr msg)
261  {
263  steering_initialized_ = true;
264  changed_ = true;
265  }
266  void receive_acceleration(std_msgs::BoolConstPtr msg)
267  {
270  changed_ = true;
271  }
272  void receive_accelerationActive(std_msgs::BoolConstPtr msg)
273  {
276  changed_ = true;
277  }
278  void receive_left_indicator(std_msgs::BoolConstPtr msg)
279  {
280  data_.setIndicatorLeftOn(msg->data);
282  changed_ = true;
283  }
284  void receive_right_indicator(std_msgs::BoolConstPtr msg)
285  {
286  data_.setIndicatorRightOn(msg->data);
288  changed_ = true;
289  }
290  void receive_checkpointClearance(std_msgs::BoolConstPtr msg)
291  {
292  data_.setCheckpointClearance(msg->data);
294  changed_ = true;
295  }
296 
297  public:
298  VehicleExtendedStateReader(ros::NodeHandle* n,
299  const std::string gear_state_topic,
300  const std::string accelerationTopic,
301  const std::string accelerationActiveTopic,
302  const std::string steeringTopic,
303  const std::string leftIndicatorTopic,
304  const std::string rightIndicatorTopic,
305  const std::string checkpointClearanceTopic,
306  int qsize)
307  :gear_initialized_(false),
308  steering_initialized_(false),
314  changed_(false)
315  {
316  bool no_delay;
317  n->param("/tcp_no_delay", no_delay, false);
318  gear_subscriber_ = n->subscribe(gear_state_topic,qsize,&VehicleExtendedStateReader::receive_gear,this,ros::TransportHints().tcpNoDelay(no_delay));
319  acceleration_subscriber_ = n->subscribe(accelerationTopic,qsize,&VehicleExtendedStateReader::receive_acceleration,this,ros::TransportHints().tcpNoDelay(no_delay));
320  accelerationActive_subscriber_ = n->subscribe(accelerationActiveTopic,qsize,&VehicleExtendedStateReader::receive_accelerationActive,this,ros::TransportHints().tcpNoDelay(no_delay));
321  steering_subscriber_ = n->subscribe(steeringTopic,qsize,&VehicleExtendedStateReader::receive_steering,this,ros::TransportHints().tcpNoDelay(no_delay));
322  leftIndicator_subscriber_ = n->subscribe(leftIndicatorTopic,qsize,&VehicleExtendedStateReader::receive_left_indicator,this,ros::TransportHints().tcpNoDelay(no_delay));
323  rightIndicator_subscriber_ = n->subscribe(rightIndicatorTopic,qsize,&VehicleExtendedStateReader::receive_right_indicator,this,ros::TransportHints().tcpNoDelay(no_delay));
324  checkpointClearance_subscriber_ = n->subscribe(checkpointClearanceTopic,qsize,&VehicleExtendedStateReader::receive_checkpointClearance,this,ros::TransportHints().tcpNoDelay(no_delay));
325  }
326  virtual bool hasData() const override
327  {
329  }
330  virtual bool hasUpdate() const override
331  {
332  return hasData() && changed_;
333  }
334  virtual void getData(adore::fun::VehicleExtendedState& value) override
335  {
336  value = data_;
337  }
338 
339  };
340 
341 
346  class VehicleExtendedStateWriter: public adore::mad::AWriter<adore::fun::VehicleExtendedState>
347  {
348  private:
349  ros::Publisher gearStatePublisher_;
350  ros::Publisher accelerationOnPublisher_;
352  ros::Publisher steeringOnPublisher_;
356  public:
357  VehicleExtendedStateWriter(ros::NodeHandle* n,
358  const std::string& gearStateTopic,
359  const std::string& accelerationTopic,
360  const std::string& accelerationActiveTopic,
361  const std::string& steeringTopic,
362  const std::string& leftIndicatorTopic,
363  const std::string& rightIndicatorTopic,
364  const std::string& checkpointClearanceTopic,
365  int qsize)
366  {
367  gearStatePublisher_ = n->advertise<std_msgs::Int8>(gearStateTopic,qsize);
368  accelerationOnPublisher_ = n->advertise<std_msgs::Bool>(accelerationTopic,qsize);
369  accelerationAcivePublisher_ = n->advertise<std_msgs::Bool>(accelerationActiveTopic,qsize);
370  steeringOnPublisher_ = n->advertise<std_msgs::Bool>(steeringTopic,qsize);
371  leftIndicatorStatePublisher_ = n->advertise<std_msgs::Bool>(leftIndicatorTopic,qsize);
372  rightIndicatorStatePublisher_ = n->advertise<std_msgs::Bool>(rightIndicatorTopic,qsize);
373  checkpointClearancePublisher_ = n->advertise<std_msgs::Bool>(checkpointClearanceTopic,qsize);
374  }
375  virtual bool canWriteMore() const override
376  {
377  return true;
378  }
380  virtual void write(const adore::fun::VehicleExtendedState& value) override
381  {
382  std_msgs::Int8 msg;
383  msg.data = (int)value.getGearState();
384  gearStatePublisher_.publish(msg);
385 
386  std_msgs::Bool bool_msg1;
387  bool_msg1.data = value.getAutomaticControlAccelerationOn();
388  accelerationOnPublisher_.publish(bool_msg1);
389 
390  bool_msg1.data = value.getAutomaticControlAccelerationActive();
391  accelerationAcivePublisher_.publish(bool_msg1);
392 
393  std_msgs::Bool bool_msg2;
394  bool_msg2.data = value.getAutomaticControlSteeringOn();
395  steeringOnPublisher_.publish(bool_msg2);
396 
397  std_msgs::Bool bool_msg3;
398  bool_msg3.data = value.getIndicatorRightOn();
399  rightIndicatorStatePublisher_.publish(bool_msg3);
400 
401  std_msgs::Bool bool_msg4;
402  bool_msg4.data = value.getIndicatorLeftOn();
403  leftIndicatorStatePublisher_.publish(bool_msg4);
404 
405  std_msgs::Bool bool_msg5;
406  bool_msg5.data = value.getCheckpointClearance();
407  checkpointClearancePublisher_.publish(bool_msg5);
408  }
409  };
410 
415  class MotionStateWriter:public adore::mad::AWriter<adore::fun::VehicleMotionState9d>
416  {
417  private:
418  ros::Publisher publisher_odom_;
419  ros::Publisher publisher_steering_;
420  ros::Publisher publisher_acceleration_;
421  tf::TransformBroadcaster broadcaster;
425  std::string frame_id_;
426 
427  public:
428  MotionStateWriter(ros::NodeHandle* n,const std::string& frame_id,const std::string& odom_topic,const std::string& steering_topic,const std::string& acceleration_topic,int qsize)
430  {
431  publisher_odom_= n->advertise<nav_msgs::Odometry>(odom_topic,qsize);
432  if(steering_topic.size()>0)
433  {
434  publisher_steering_ = n->advertise<std_msgs::Float32>(steering_topic,qsize);
436  }
437  if(acceleration_topic.size()>0)
438  {
439  publisher_acceleration_ = n->advertise<std_msgs::Float32>(acceleration_topic,qsize);
441  }
442  params_=nullptr;
443  }
444  virtual bool canWriteMore() const override
445  {
446  return true;//@TODO
447  }
448  virtual void write(const adore::fun::VehicleMotionState9d& value)override
449  {
450  if(params_==nullptr)
451  {
453  if(pfactory==nullptr)
454  {
455  std::cerr << " WARNING not writing motion state until parameter factory and steeringRatio parameter are available."
456  << std::endl;
457  return;
458  }
459  else
460  {
461  params_ = pfactory->getVehicle();
462  }
463  }
464  ros::Time now;
465  now.fromSec(value.getTime());
466  nav_msgs::Odometry odom_msg;
467  odom_msg.header.stamp=now;
468  odom_msg.header.frame_id = frame_id_;
469  odom_msg.child_frame_id = "base_link";
470  odom_msg.pose.pose.position.x = value.getX();
471  odom_msg.pose.pose.position.y = value.getY();
472  odom_msg.pose.pose.position.z = value.getZ();
474  qc.setHeading(value.getPSI(),odom_msg.pose.pose);
475  odom_msg.twist.twist.linear.x = value.getvx();
476  odom_msg.twist.twist.linear.y = value.getvy();
477  odom_msg.twist.twist.linear.z = 0.0;
478  odom_msg.twist.twist.angular.x =0.0;
479  odom_msg.twist.twist.angular.y =0.0;
480  odom_msg.twist.twist.angular.z =value.getOmega();
481  publisher_odom_.publish(odom_msg);
482  std_msgs::Float32 delta;
483  delta.data = value.getDelta() * params_->get_steeringRatio();
485  std_msgs::Float32 acceleration;
486  acceleration.data = value.getAx();
488  // broadcaster.sendTransform(
489  // tf::StampedTransform(
490  // tf::Transform(tf::Quaternion(0,0,0,1),tf::Vector3(0.0,0.0,0.0)),
491  // now,"map","UTMZone"
492  // )
493  // );
494  }
495  };
496  }
497 }
Definition: vehicleextendedstate.h:26
bool getIndicatorLeftOn() const
Definition: vehicleextendedstate.h:59
bool getAutomaticControlAccelerationOn() const
Definition: vehicleextendedstate.h:75
void setGearState(GearState gearState)
Definition: vehicleextendedstate.h:54
void setCheckpointClearance(bool checkpointClearance)
Definition: vehicleextendedstate.h:106
void setAutomaticControlAccelerationActive(bool automaticControlAccelerationActive)
Definition: vehicleextendedstate.h:86
GearState
Definition: vehicleextendedstate.h:29
bool getCheckpointClearance() const
Definition: vehicleextendedstate.h:103
void setIndicatorRightOn(bool indicatorRightOn)
Definition: vehicleextendedstate.h:70
void setAutomaticControlAccelerationOn(bool automaticControlAccelerationOn)
Definition: vehicleextendedstate.h:78
void setIndicatorLeftOn(bool indicatorLeftOn)
Definition: vehicleextendedstate.h:62
void setAutomaticControlSteeringOn(bool automaticControlSteeringOn)
Definition: vehicleextendedstate.h:94
bool getAutomaticControlSteeringOn() const
Definition: vehicleextendedstate.h:91
bool getAutomaticControlAccelerationActive() const
Definition: vehicleextendedstate.h:83
GearState getGearState() const
Definition: vehicleextendedstate.h:51
bool getIndicatorRightOn() const
Definition: vehicleextendedstate.h:67
Definition: funvehiclemotionstateconverter.h:139
ros::Subscriber acceleration_subscriber_
Definition: funvehiclemotionstateconverter.h:143
double delta_
Definition: funvehiclemotionstateconverter.h:145
std::list< adore::fun::VehicleMotionState9d > queue_
Definition: funvehiclemotionstateconverter.h:147
MotionStateFeed(ros::NodeHandle *n, const std::string odom_topic, const std::string accelerationTopic, const std::string steeringAngleTopic, int qsize)
Definition: funvehiclemotionstateconverter.h:189
adore::params::APVehicle * params_
Definition: funvehiclemotionstateconverter.h:146
ros::Subscriber steering_subscriber_
Definition: funvehiclemotionstateconverter.h:142
void receive_odom(nav_msgs::OdometryConstPtr msg)
Definition: funvehiclemotionstateconverter.h:149
virtual bool hasNext() const override
Definition: funvehiclemotionstateconverter.h:207
void receive_steering(std_msgs::Float32ConstPtr msg)
Definition: funvehiclemotionstateconverter.h:165
ros::Subscriber odom_subscriber_
Definition: funvehiclemotionstateconverter.h:141
double ax_
Definition: funvehiclemotionstateconverter.h:144
virtual void getNext(adore::fun::VehicleMotionState9d &value) override
Definition: funvehiclemotionstateconverter.h:214
virtual void getLatest(adore::fun::VehicleMotionState9d &value) override
Definition: funvehiclemotionstateconverter.h:222
void receive_acceleration(std_msgs::Float32ConstPtr msg)
Definition: funvehiclemotionstateconverter.h:183
Definition: funvehiclemotionstateconverter.h:47
void receive_acceleration(std_msgs::Float32ConstPtr msg)
Definition: funvehiclemotionstateconverter.h:94
void receive_steering(std_msgs::Float32ConstPtr msg)
Definition: funvehiclemotionstateconverter.h:74
ros::Subscriber acceleration_subscriber_
Definition: funvehiclemotionstateconverter.h:51
bool steering_initialized_
Definition: funvehiclemotionstateconverter.h:53
void receive_odom(nav_msgs::OdometryConstPtr msg)
Definition: funvehiclemotionstateconverter.h:60
MotionStateReader(ros::NodeHandle *n, const std::string odom_topic, const std::string accelerationTopic, const std::string steeringAngleTopic, int qsize)
Definition: funvehiclemotionstateconverter.h:102
ros::Subscriber steering_subscriber_
Definition: funvehiclemotionstateconverter.h:50
bool acceleration_initialized_
Definition: funvehiclemotionstateconverter.h:54
bool changed_
Definition: funvehiclemotionstateconverter.h:56
virtual bool hasData() const override
Definition: funvehiclemotionstateconverter.h:119
bool wheel_speed_available_
Definition: funvehiclemotionstateconverter.h:55
virtual void getData(adore::fun::VehicleMotionState9d &value) override
Definition: funvehiclemotionstateconverter.h:127
ros::Subscriber odom_subscriber_
Definition: funvehiclemotionstateconverter.h:49
adore::params::APVehicle * params_
Definition: funvehiclemotionstateconverter.h:58
bool odom_initialized_
Definition: funvehiclemotionstateconverter.h:52
virtual bool hasUpdate() const override
Definition: funvehiclemotionstateconverter.h:123
adore::fun::VehicleMotionState9d data_
Definition: funvehiclemotionstateconverter.h:57
Definition: funvehiclemotionstateconverter.h:416
virtual bool canWriteMore() const override
Definition: funvehiclemotionstateconverter.h:444
std::string frame_id_
Definition: funvehiclemotionstateconverter.h:425
MotionStateWriter(ros::NodeHandle *n, const std::string &frame_id, const std::string &odom_topic, const std::string &steering_topic, const std::string &acceleration_topic, int qsize)
Definition: funvehiclemotionstateconverter.h:428
ros::Publisher publisher_odom_
Definition: funvehiclemotionstateconverter.h:418
bool publisher_steering_initialized_
Definition: funvehiclemotionstateconverter.h:423
bool publisher_acceleration_initialized_
Definition: funvehiclemotionstateconverter.h:424
ros::Publisher publisher_acceleration_
Definition: funvehiclemotionstateconverter.h:420
ros::Publisher publisher_steering_
Definition: funvehiclemotionstateconverter.h:419
tf::TransformBroadcaster broadcaster
Definition: funvehiclemotionstateconverter.h:421
adore::params::APVehicle * params_
Definition: funvehiclemotionstateconverter.h:422
virtual void write(const adore::fun::VehicleMotionState9d &value) override
Definition: funvehiclemotionstateconverter.h:448
Definition: funvehiclemotionstateconverter.h:235
ros::Subscriber checkpointClearance_subscriber_
Definition: funvehiclemotionstateconverter.h:243
bool gear_initialized_
Definition: funvehiclemotionstateconverter.h:244
void receive_acceleration(std_msgs::BoolConstPtr msg)
Definition: funvehiclemotionstateconverter.h:266
ros::Subscriber gear_subscriber_
Definition: funvehiclemotionstateconverter.h:237
VehicleExtendedStateReader(ros::NodeHandle *n, const std::string gear_state_topic, const std::string accelerationTopic, const std::string accelerationActiveTopic, const std::string steeringTopic, const std::string leftIndicatorTopic, const std::string rightIndicatorTopic, const std::string checkpointClearanceTopic, int qsize)
Definition: funvehiclemotionstateconverter.h:298
bool changed_
Definition: funvehiclemotionstateconverter.h:251
void receive_checkpointClearance(std_msgs::BoolConstPtr msg)
Definition: funvehiclemotionstateconverter.h:290
void receive_steering(std_msgs::BoolConstPtr msg)
Definition: funvehiclemotionstateconverter.h:260
bool acceleration_initialized_
Definition: funvehiclemotionstateconverter.h:246
ros::Subscriber acceleration_subscriber_
Definition: funvehiclemotionstateconverter.h:239
virtual bool hasData() const override
Definition: funvehiclemotionstateconverter.h:326
bool steering_initialized_
Definition: funvehiclemotionstateconverter.h:245
bool checkpointClearance_initialized_
Definition: funvehiclemotionstateconverter.h:250
void receive_accelerationActive(std_msgs::BoolConstPtr msg)
Definition: funvehiclemotionstateconverter.h:272
ros::Subscriber accelerationActive_subscriber_
Definition: funvehiclemotionstateconverter.h:240
void receive_gear(std_msgs::Int8ConstPtr msg)
Definition: funvehiclemotionstateconverter.h:254
ros::Subscriber leftIndicator_subscriber_
Definition: funvehiclemotionstateconverter.h:241
adore::fun::VehicleExtendedState data_
Definition: funvehiclemotionstateconverter.h:252
bool right_indicator_initialized_
Definition: funvehiclemotionstateconverter.h:249
void receive_left_indicator(std_msgs::BoolConstPtr msg)
Definition: funvehiclemotionstateconverter.h:278
ros::Subscriber rightIndicator_subscriber_
Definition: funvehiclemotionstateconverter.h:242
ros::Subscriber steering_subscriber_
Definition: funvehiclemotionstateconverter.h:238
virtual bool hasUpdate() const override
Definition: funvehiclemotionstateconverter.h:330
bool left_indicator_initialized_
Definition: funvehiclemotionstateconverter.h:248
virtual void getData(adore::fun::VehicleExtendedState &value) override
Definition: funvehiclemotionstateconverter.h:334
void receive_right_indicator(std_msgs::BoolConstPtr msg)
Definition: funvehiclemotionstateconverter.h:284
bool accelerationActive_initialized_
Definition: funvehiclemotionstateconverter.h:247
Definition: funvehiclemotionstateconverter.h:347
VehicleExtendedStateWriter(ros::NodeHandle *n, const std::string &gearStateTopic, const std::string &accelerationTopic, const std::string &accelerationActiveTopic, const std::string &steeringTopic, const std::string &leftIndicatorTopic, const std::string &rightIndicatorTopic, const std::string &checkpointClearanceTopic, int qsize)
Definition: funvehiclemotionstateconverter.h:357
ros::Publisher checkpointClearancePublisher_
Definition: funvehiclemotionstateconverter.h:355
virtual void write(const adore::fun::VehicleExtendedState &value) override
write sends out data value
Definition: funvehiclemotionstateconverter.h:380
ros::Publisher accelerationAcivePublisher_
Definition: funvehiclemotionstateconverter.h:351
ros::Publisher gearStatePublisher_
Definition: funvehiclemotionstateconverter.h:349
ros::Publisher accelerationOnPublisher_
Definition: funvehiclemotionstateconverter.h:350
ros::Publisher steeringOnPublisher_
Definition: funvehiclemotionstateconverter.h:352
virtual bool canWriteMore() const override
Definition: funvehiclemotionstateconverter.h:375
ros::Publisher rightIndicatorStatePublisher_
Definition: funvehiclemotionstateconverter.h:354
ros::Publisher leftIndicatorStatePublisher_
Definition: funvehiclemotionstateconverter.h:353
Definition: com_patterns.h:29
Definition: com_patterns.h:68
Definition: com_patterns.h:97
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
virtual double get_steeringRatio() const =0
static adore::params::AFactory * get()
Definition: afactory.h:103
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
void setZ(double value)
Set the z-coordinate.
Definition: vehiclemotionstate9d.h:127
void setY(double value)
Set the y-coordinate.
Definition: vehiclemotionstate9d.h:121
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getvx() const
Get the longitudinal velocity.
Definition: vehiclemotionstate9d.h:78
void setAx(double value)
Set the longitudinal acceleration.
Definition: vehiclemotionstate9d.h:157
double getOmega() const
Get the yaw rate.
Definition: vehiclemotionstate9d.h:90
void setvx(double value)
set the longitudinal velocity
Definition: vehiclemotionstate9d.h:139
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72
double getAx() const
Get the longitudinal acceleration.
Definition: vehiclemotionstate9d.h:96
void setOmega(double value)
Set the yaw rate.
Definition: vehiclemotionstate9d.h:151
double getvy() const
Get the lateral velocity.
Definition: vehiclemotionstate9d.h:84
void setX(double value)
Set the x-coordinate.
Definition: vehiclemotionstate9d.h:115
double getZ() const
Get the z-coordinate.
Definition: vehiclemotionstate9d.h:66
double getDelta() const
Get the steering angle.
Definition: vehiclemotionstate9d.h:102
void setPSI(double value)
set the heading
Definition: vehiclemotionstate9d.h:133
void setvy(double value)
set the lateral velocity
Definition: vehiclemotionstate9d.h:145
void setDelta(double value)
Set the steering angle.
Definition: vehiclemotionstate9d.h:163
void setTime(double value)
Set the time.
Definition: vehiclemotionstate9d.h:109
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48
Definition: quaternion.h:28
double quaternionToHeading(const geometry_msgs::Pose &p) const
convert quaternion to heading
Definition: quaternion.h:49
void setHeading(double heading, geometry_msgs::Pose &msg) const
set heading of a Pose message
Definition: quaternion.h:31