Go to the documentation of this file.00001 #include <vigir_foot_pose_transformer/foot_pose_transformer.h>
00002
00003 namespace vigir_footstep_planning
00004 {
00005 FootPoseTransformer::FootPoseTransformer(ros::NodeHandle& nh)
00006 {
00007
00008 geometry_msgs::Vector3 left_origin;
00009 getXYZ(nh, "foot/left/foot_frame", left_origin);
00010 tf::Vector3 left_origin_tf;
00011 tf::vector3MsgToTF(left_origin, left_origin_tf);
00012 left_foot_frame_transform.setOrigin(left_origin_tf);
00013
00014 geometry_msgs::Vector3 left_orientation;
00015 getRPY(nh, "foot/left/foot_frame", left_orientation);
00016 left_foot_frame_transform.setRotation(tf::createQuaternionFromRPY(left_orientation.x, left_orientation.y, left_orientation.z));
00017
00018
00019 geometry_msgs::Vector3 right_origin;
00020 getXYZ(nh, "foot/right/foot_frame", right_origin);
00021 tf::Vector3 right_origin_tf;
00022 tf::vector3MsgToTF(right_origin, right_origin_tf);
00023 right_foot_frame_transform.setOrigin(right_origin_tf);
00024
00025 geometry_msgs::Vector3 right_orientation;
00026 getRPY(nh, "foot/right/foot_frame", right_orientation);
00027 right_foot_frame_transform.setRotation(tf::createQuaternionFromRPY(right_orientation.x, right_orientation.y, right_orientation.z));
00028 }
00029
00030 FootPoseTransformer::~FootPoseTransformer()
00031 {
00032 }
00033
00034 msgs::ErrorStatus FootPoseTransformer::transform(geometry_msgs::Pose& pose, const tf::Transform& transform) const
00035 {
00036 tf::Pose pose_tf;
00037 tf::poseMsgToTF(pose, pose_tf);
00038 pose_tf = pose_tf * transform;
00039 tf::poseTFToMsg(pose_tf, pose);
00040 return msgs::ErrorStatus();
00041 }
00042
00043 msgs::ErrorStatus FootPoseTransformer::transform(msgs::Foot& foot, const std::string& target_frame) const
00044 {
00045 if (foot.foot_index != msgs::Foot::LEFT && foot.foot_index != msgs::Foot::RIGHT)
00046 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "FootPoseTransformer", "Foot index '" + boost::lexical_cast<std::string>(foot.foot_index) + "' is unknown!");
00047
00048 if (target_frame == "planner")
00049 return transform(foot.pose, foot.foot_index == msgs::Foot::LEFT ? left_foot_frame_transform : right_foot_frame_transform);
00050 else if (target_frame == "robot")
00051 return transform(foot.pose, foot.foot_index == msgs::Foot::LEFT ? left_foot_frame_transform.inverse() : right_foot_frame_transform.inverse());
00052 else
00053 return ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN, "FootPoseTransformer", "Target frame '" + target_frame + "' is unknown!");
00054 }
00055
00056 msgs::ErrorStatus FootPoseTransformer::transform(msgs::Feet& feet, const std::string& target_frame) const
00057 {
00058 msgs::ErrorStatus status;
00059 status += transform(feet.left, target_frame);
00060 status += transform(feet.right, target_frame);
00061 return status;
00062 }
00063
00064 msgs::ErrorStatus FootPoseTransformer::transform(msgs::Step& step, const std::string& target_frame) const
00065 {
00066 return transform(step.foot, target_frame);
00067 }
00068
00069 msgs::ErrorStatus FootPoseTransformer::transform(msgs::StepPlan& step_plan, const std::string& target_frame) const
00070 {
00071 msgs::ErrorStatus status;
00072 status += transform(step_plan.steps, target_frame);
00073 status += transform(step_plan.start.left, target_frame);
00074 status += transform(step_plan.start.right, target_frame);
00075 status += transform(step_plan.goal.left, target_frame);
00076 status += transform(step_plan.goal.right, target_frame);
00077 return status;
00078 }
00079 }