wing_moveit_decorator.cpp
Go to the documentation of this file.
2 
3 void Wing_moveit_decorator::update(tf2::Transform& tf) {
6 }
7 
8 void Wing_moveit_decorator::input_filter(tf2::Transform& tf) {}
9 
11  Wing* wing = dynamic_cast<Wing*> (next_);
12  tf2::Vector3 world_origin = wing->world_tf().getOrigin();
13  tf2::Quaternion world_quat = wing->world_tf().getRotation().normalized();
14 
15 
16  markers_->id = wing->name();
17  markers_->header.frame_id = "world";
18 
19  markers_->primitives.resize(1);
20  markers_->primitives[0].type = markers_->primitives[0].BOX;
21  markers_->primitives[0].dimensions.resize(3);
22  markers_->primitives[0].dimensions[0] = wing->size().getX();
23  markers_->primitives[0].dimensions[1] = wing->size().getY();
24  markers_->primitives[0].dimensions[2] = wing->size().getZ();
25 
26 
27  markers_->primitive_poses.resize(1);
28  markers_->primitive_poses[0].position.x = world_origin.getX();
29  markers_->primitive_poses[0].position.y = world_origin.getY();
30  markers_->primitive_poses[0].position.z = world_origin.getZ() - wing->size().getZ()/2;
31  markers_->primitive_poses[0].orientation.x = world_quat.getX();
32  markers_->primitive_poses[0].orientation.y = world_quat.getY();
33  markers_->primitive_poses[0].orientation.z = world_quat.getZ();
34  markers_->primitive_poses[0].orientation.w = world_quat.getW();
35 
36  markers_->operation = markers_->ADD;
37 }
Wing_moveit_decorator::markers_
moveit_msgs::CollisionObject * markers_
Definition: wing_moveit_decorator.h:15
Abstract_robot_element::name
virtual std::string & name()=0
Abstract_robot_element::size
virtual tf2::Vector3 & size()=0
Wing_moveit_decorator::update
void update(tf2::Transform &tf) override
Definition: wing_moveit_decorator.cpp:3
wing_moveit_decorator.h
Abstract_robot_element_decorator::wing
Abstract_robot_element * wing()
Definition: impl/abstract_robot_element_decorator.h:15
Abstract_robot_element_decorator::update
void update(tf2::Transform &tf) override
Definition: impl/abstract_robot_element_decorator.h:17
Wing_moveit_decorator::output_filter
void output_filter() override
Definition: wing_moveit_decorator.cpp:10
Abstract_robot_element_decorator::next_
Abstract_robot_element * next_
Definition: impl/abstract_robot_element_decorator.h:9
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