18 #include <tf/transform_broadcaster.h>
19 #include <tf/LinearMath/Quaternion.h>
20 #include <tf2/LinearMath/Matrix3x3.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>
81 std::cerr <<
" WARNING not reading motion state (steering angle) until parameter factory and steeringRatio parameter are available."
87 params_ = pfactory->getVehicle();
103 const std::string odom_topic,
104 const std::string accelerationTopic,
105 const std::string steeringAngleTopic,
113 n->param(
"/tcp_no_delay", no_delay,
false);
147 std::list<adore::fun::VehicleMotionState9d>
queue_;
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);
158 data.
setvx(msg->twist.twist.linear.x);
159 data.
setvy(msg->twist.twist.linear.y);
160 data.
setOmega(msg->twist.twist.angular.z);
170 if(pfactory==
nullptr)
172 std::cerr <<
" WARNING not reading motion state (steering angle) until parameter factory and steeringRatio parameter are available."
178 params_ = pfactory->getVehicle();
190 const std::string odom_topic,
191 const std::string accelerationTopic,
192 const std::string steeringAngleTopic,
198 n->param(
"/tcp_no_delay", no_delay,
false);
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,
317 n->param(
"/tcp_no_delay", no_delay,
false);
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,
386 std_msgs::Bool bool_msg1;
393 std_msgs::Bool bool_msg2;
397 std_msgs::Bool bool_msg3;
401 std_msgs::Bool bool_msg4;
405 std_msgs::Bool bool_msg5;
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)
432 if(steering_topic.size()>0)
437 if(acceleration_topic.size()>0)
453 if(pfactory==
nullptr)
455 std::cerr <<
" WARNING not writing motion state until parameter factory and steeringRatio parameter are available."
461 params_ = pfactory->getVehicle();
466 nav_msgs::Odometry odom_msg;
467 odom_msg.header.stamp=now;
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();
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();
482 std_msgs::Float32 delta;
485 std_msgs::Float32 acceleration;
486 acceleration.data = value.
getAx();
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