12 tf2::Vector3 world_origin = field->
world_tf().getOrigin();
13 tf2::Quaternion world_quat = field->
world_tf().getRotation().normalized();
15 visualization_msgs::Marker marker;
16 marker.header.frame_id =
"map";
17 marker.header.stamp = ros::Time();
19 marker.id = *((
int*)(&
next_));
20 marker.type = visualization_msgs::Marker::CUBE;
21 marker.action = visualization_msgs::Marker::ADD;
22 marker.pose.position.x = world_origin.getX();
23 marker.pose.position.y = world_origin.getY();
24 marker.pose.position.z = 0;
25 marker.pose.orientation.x = world_quat.getX();
26 marker.pose.orientation.y = world_quat.getY();
27 marker.pose.orientation.z = world_quat.getZ();
28 marker.pose.orientation.w = world_quat.getW();
29 marker.scale.x = 0.4f;
30 marker.scale.y = 0.4f;
31 marker.scale.z = 0.01f;