8 geometry_msgs::Vector3 left_origin;
9 getXYZ(nh,
"foot/left/foot_frame", left_origin);
14 geometry_msgs::Vector3 left_orientation;
15 getRPY(nh,
"foot/left/foot_frame", left_orientation);
19 geometry_msgs::Vector3 right_origin;
20 getXYZ(nh,
"foot/right/foot_frame", right_origin);
25 geometry_msgs::Vector3 right_orientation;
26 getRPY(nh,
"foot/right/foot_frame", right_orientation);
40 return msgs::ErrorStatus();
45 if (foot.foot_index != msgs::Foot::LEFT && foot.foot_index != msgs::Foot::RIGHT)
46 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootPoseTransformer",
"Foot index '" + boost::lexical_cast<std::string>(foot.foot_index) +
"' is unknown!");
48 if (target_frame ==
"planner")
50 else if (target_frame ==
"robot")
53 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FootPoseTransformer",
"Target frame '" + target_frame +
"' is unknown!");
58 msgs::ErrorStatus status;
59 status +=
transform(feet.left, target_frame);
60 status +=
transform(feet.right, target_frame);
66 return transform(step.foot, target_frame);
71 msgs::ErrorStatus status;
72 status +=
transform(step_plan.steps, target_frame);
73 status +=
transform(step_plan.start.left, target_frame);
74 status +=
transform(step_plan.start.right, target_frame);
75 status +=
transform(step_plan.goal.left, target_frame);
76 status +=
transform(step_plan.goal.right, target_frame);
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)