17 #include <nav_msgs/Odometry.h>
18 #include <tf2/LinearMath/Quaternion.h>
19 #include <tf/LinearMath/Matrix3x3.h>
20 #include <geometry_msgs/Pose.h>
21 #include <geometry_msgs/Twist.h>
41 tf::Matrix3x3(tf::Quaternion(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w)).getRPY(
r,p,
y);
42 pose.
setX(msg->position.x);
43 pose.
setY(msg->position.y);
44 pose.
setZ(msg->position.z);
52 geometry_msgs::Pose msg;
53 msg.position.x = pose.
getX();
54 msg.position.y = pose.
getY();
55 msg.position.z = pose.
getZ();
57 q.setRPY(0.0, 0.0, pose.
getPSI());
58 msg.orientation.w = q.getW();
59 msg.orientation.x = q.getX();
60 msg.orientation.y = q.getY();
61 msg.orientation.z = q.getZ();
69 twist.
setVx(msg->linear.x);
70 twist.
setVy(msg->linear.y);
79 geometry_msgs::Twist msg;
80 msg.linear.x = twist.
getVx();
81 msg.linear.y = twist.
getVy();
y
Definition: adore_set_goal.py:31
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
Definition: simvehicleresetconverter.h:33
geometry_msgs::Twist operator()(const adore::sim::ResetVehicleTwist &twist)
Definition: simvehicleresetconverter.h:77
void operator()(geometry_msgs::PoseConstPtr msg, adore::sim::ResetVehiclePose &pose)
Definition: simvehicleresetconverter.h:38
void operator()(geometry_msgs::TwistConstPtr msg, adore::sim::ResetVehicleTwist &twist)
Definition: simvehicleresetconverter.h:67
geometry_msgs::Pose operator()(const adore::sim::ResetVehiclePose &pose)
Definition: simvehicleresetconverter.h:50
provides encapsulation of values needed to reset the vehicle pose in a simulation
Definition: resetvehiclepose.h:26
void setZ(double Z)
Definition: resetvehiclepose.h:61
void setY(double Y)
Definition: resetvehiclepose.h:46
double getZ() const
Definition: resetvehiclepose.h:58
double getX() const
Definition: resetvehiclepose.h:35
void setPSI(double PSI)
Definition: resetvehiclepose.h:54
double getY() const
Definition: resetvehiclepose.h:43
void setX(double X)
Definition: resetvehiclepose.h:38
double getPSI() const
Definition: resetvehiclepose.h:51
provides encapsulation of values needed to reset the vehicle twist (vx,vy and omega) in a simulation
Definition: resetvehicletwist.h:26
double getVy() const
Definition: resetvehicletwist.h:41
void setOmega(double omega)
Definition: resetvehicletwist.h:53
void setVx(double vx)
Definition: resetvehicletwist.h:35
void setVy(double vy)
Definition: resetvehicletwist.h:44
double getVx() const
Definition: resetvehicletwist.h:32
double getOmega() const
Definition: resetvehicletwist.h:50