8 root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0,
tf.getOrigin().getZ()));
10 bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(
size.getX()*0.5f,
size.getY()*0.5f,
tf.getOrigin().getZ())));
11 bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-
size.getX()*0.5f,
size.getY()*0.5f,
tf.getOrigin().getZ())));
12 bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-
size.getX()*0.5f, -
size.getY()*0.5f,
tf.getOrigin().getZ())));
13 bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(
size.getX()*0.5f, -
size.getY()*0.5f,
tf.getOrigin().getZ())));
16 robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.1f, 0.1f, 0)));
17 robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.1f, 0.1f, 0)));
18 robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.1f, -0.1f, 0)));
19 robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.1f, -0.1f, 0)));
24 (B.getOrigin().getX() * A.getOrigin().getY()) - (A.getOrigin().getX() * B.getOrigin().getY()) +
25 (C.getOrigin().getX() * B.getOrigin().getY()) - (B.getOrigin().getX() * C.getOrigin().getY()) +
26 (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;