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); }