static inline geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw) {
geometry_msgs::Quaternion q;
q = setRPY(0.0, 0.0, yaw);
geometry_msgs::Quaternion q_msg;
quaternionTFToMsg(q, q_msg);
return q_msg;
}
void publishCurrentPose(double x, double y, double yaw) {
geometry_msgs::PoseStamped pose;
pose.pose.position.x = x;
pose.pose.position.y = y;
tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, yaw);
quaternionTFToMsg(quaternion, pose.pose.orientation);
current_pose_pub.publish(pose);
}
/**
* Propogates the dynamics for 1 step
*/
Pose propogateDynamics(Pose start, float speed, float turnRate) {
Pose result = start;
double roll, pitch, yaw;
tf::Quaternion bt_q;
quaternionMsgToTF(start.orientation, bt_q);
tf::Matrix3x3(bt_q).getRPY(roll, pitch, yaw);
result.position.x += CYCLE_TIME * speed * cos(yaw);
result.position.y += CYCLE_TIME * speed * sin(yaw);
yaw += CYCLE_TIME * turnRate;
quaternionTFToMsg(
tf::createQuaternionFromRPY(roll, pitch, yaw),
result.orientation);
return result;
}
void publishTrafficWaypointsArray() {
autoware_msgs::LaneArray pub_msg;
autoware_msgs::Lane pub_lane;
for (int idx = 0; idx < 100; idx++) {
static autoware_msgs::Waypoint wp;
wp.gid = idx;
wp.lid = idx;
wp.pose.pose.position.x = 0.0 + (double)idx;
wp.pose.pose.position.y = 0.0;
wp.pose.pose.position.z = 0.0;
wp.twist.twist.linear.x = 5.0;
wp.twist.twist.angular.z = 0.0;
tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, 0);
quaternionTFToMsg(quaternion, wp.pose.pose.orientation);
pub_lane.waypoints.push_back(wp);
}
pub_msg.lanes.push_back(pub_lane);
pub_msg.id = lane_array_id_;
traffic_waypoints_array_pub.publish(pub_msg);
}