10 tf2::Vector3 world_origin = tf.getOrigin();
11 tf2::Quaternion world_quat = tf.getRotation().normalized();
13 visualization_msgs::Marker marker;
14 marker.header.frame_id =
"map";
15 marker.header.stamp = ros::Time();
18 marker.type = visualization_msgs::Marker::CUBE;
19 marker.action = visualization_msgs::Marker::ADD;
20 marker.pose.position.x = world_origin.getX();
21 marker.pose.position.y = world_origin.getY();
22 marker.pose.position.z = world_origin.getZ();
23 marker.pose.orientation.x = world_quat.getX();
24 marker.pose.orientation.y = world_quat.getY();
25 marker.pose.orientation.z = world_quat.getZ();
26 marker.pose.orientation.w = world_quat.getW();
27 marker.scale.x = 0.8f;
28 marker.scale.y = 0.8f;
29 marker.scale.z = 0.885f;
38 tf2::Vector3 world_origin =
wing->
world_tf().getOrigin();
39 tf2::Quaternion world_quat =
wing->
world_tf().getRotation().normalized();
41 marker_->header.frame_id =
"map";
42 marker_->header.stamp = ros::Time();
45 marker_->type = visualization_msgs::Marker::CUBE;
46 marker_->action = visualization_msgs::Marker::MODIFY;
47 marker_->pose.position.x = world_origin.getX();
48 marker_->pose.position.y = world_origin.getY();
49 marker_->pose.position.z = world_origin.getZ();
50 marker_->pose.orientation.x = world_quat.getX();
51 marker_->pose.orientation.y = world_quat.getY();
52 marker_->pose.orientation.z = world_quat.getZ();
53 marker_->pose.orientation.w = world_quat.getW();