wing_rviz_decorator.cpp
Go to the documentation of this file.
2 
3 void Wing_rviz_decorator::update(tf2::Transform& tf) {
4  //input_filter(tf);
7 }
8 
9 void Wing_rviz_decorator::input_filter(tf2::Transform& tf) {
10  tf2::Vector3 world_origin = tf.getOrigin();
11  tf2::Quaternion world_quat = tf.getRotation().normalized();
12 
13  visualization_msgs::Marker marker;
14  marker.header.frame_id = "map";
15  marker.header.stamp = ros::Time();
16  marker.ns = "";
17  marker.id = 1;
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;
30  marker.color.r = 1.0;
31  marker.color.g = 1.0;
32  marker.color.b = 1.0;
33  marker.color.a = 1.0; // Don't forget to set the alpha
34 }
35 
37  Wing* wing = dynamic_cast<Wing*> (next_);
38  tf2::Vector3 world_origin = wing->world_tf().getOrigin();
39  tf2::Quaternion world_quat = wing->world_tf().getRotation().normalized();
40 
41  marker_->header.frame_id = "map";
42  marker_->header.stamp = ros::Time();
43  marker_->ns = wing->name();
44  marker_->id = 1;
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();
54  marker_->scale.x = wing->size().getX();
55  marker_->scale.y = wing->size().getY();
56  marker_->scale.z = wing->size().getZ();
57  marker_->color.r = 1.0;
58  marker_->color.g = 1.0;
59  marker_->color.b = 1.0;
60  marker_->color.a = 1.0; // Don't forget to set the alpha!
61  /*
62  visualization_msgs::Marker bounds;
63  bounds.header.frame_id = "map";
64  bounds.header.stamp = ros::Time();
65  bounds.ns = "bounds";
66  bounds.id = *((int*)(&wing));
67  bounds.type = visualization_msgs::Marker::POINTS;
68  bounds.action = visualization_msgs::Marker::MODIFY;
69  bounds.scale.x = 0.01f;
70  bounds.scale.y = 0.01f;
71  bounds.scale.z = 0.01f;
72  bounds.color.r = 0.0;
73  bounds.color.g = 0.0;
74  bounds.color.b = 1.0;
75  bounds.color.a = 1.0;
76  bounds.pose.orientation.w = 1;
77 
78  for (long unsigned int i = 0; i < wing->bounds().size(); i++){
79  tf2::Transform point_posistion = wing->world_tf() * wing->bounds()[i];
80  geometry_msgs::Point point;
81  point.x = point_posistion.getOrigin().getX();
82  point.y = point_posistion.getOrigin().getY();
83  point.z = point_posistion.getOrigin().getZ();
84  bounds.points.push_back(point);
85  }
86  */
87 }
88 
Wing_rviz_decorator::output_filter
void output_filter() override
Definition: wing_rviz_decorator.cpp:36
Abstract_robot_element::name
virtual std::string & name()=0
Wing_rviz_decorator::update
void update(tf2::Transform &tf) override
Definition: wing_rviz_decorator.cpp:3
Abstract_robot_element::size
virtual tf2::Vector3 & size()=0
Abstract_robot_element_decorator::wing
Abstract_robot_element * wing()
Definition: impl/abstract_robot_element_decorator.h:15
Wing_rviz_decorator::marker_
visualization_msgs::Marker * marker_
Definition: wing_rviz_decorator.h:11
Abstract_robot_element_decorator::update
void update(tf2::Transform &tf) override
Definition: impl/abstract_robot_element_decorator.h:17
Abstract_robot_element_decorator::next_
Abstract_robot_element * next_
Definition: impl/abstract_robot_element_decorator.h:9
wing_rviz_decorator.h
Abstract_robot_element::world_tf
tf2::Transform & world_tf()
Definition: impl/abstract_robot_element.h:21
Abstract_robot_element_decorator::input_filter
virtual void input_filter()=0
Wing
Definition: wing.h:7


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43