6 : has_robot_pose(false)
34 this->robot_pose.header = robot_pose->header;
41 this->robot_pose.header = robot_pose->header;
42 this->robot_pose.pose = robot_pose->pose.pose;
49 if (this->terrain_model)
50 this->terrain_model->fromMsg(*terrain_model);
52 this->terrain_model.reset(
new vigir_terrain_classifier::TerrainModel(*terrain_model));
57 msgs::ErrorStatus status;
60 std::string request_frame_id = request.header.frame_id;
61 strip(request_frame_id,
'/');
63 if (request.flags & msgs::FeetPoseRequest::FLAG_CURRENT)
73 std::string robot_pose_frame_id = strip_const(
robot_pose.header.frame_id,
'/');
74 if (request_frame_id != robot_pose_frame_id)
75 status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FeetPoseGenerator",
"generateFeetPose: Frame ID of robot pose ('" + robot_pose_frame_id +
"') and request ('" + request_frame_id +
"') mismatch, automatic transformation is not implemented yet!");
82 return status + ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FeetPoseGenerator",
"generateFeetPose: No robot pose available for handling FLAG_CURRENT!");
87 geometry_msgs::PoseStamped pose;
88 pose.header = request.header;
89 pose.pose = request.pose;
93 if (request.flags & msgs::FeetPoseRequest::FLAG_PELVIS_FRAME)
98 if (request.flags & msgs::FeetPoseRequest::FLAG_3D)
100 msgs::ErrorStatus temp_status;
101 msgs::Feet temp_feet = feet;
104 if (hasError(temp_status))
105 status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FeetPoseGenerator",
"generateFeetPose: Couldn't determine either 3D nor height of feet!",
false);
108 status += temp_status;
113 if (request.flags & msgs::FeetPoseRequest::FLAG_CURRENT_Z)
115 msgs::Feet current_feet;
122 status += ErrorStatusError(msgs::ErrorStatus::ERR_UNKNOWN,
"FeetPoseGenerator",
"generateFeetPose: Couldn't determine height of current feet!");
133 return ErrorStatusError(msgs::ErrorStatus::ERR_INVALID_TERRAIN_MODEL,
"FeetPoseGenerator",
"updateFeetPose: No terrain model available!",
false);
135 msgs::ErrorStatus status;
137 if (!
terrain_model->update3DData(feet.left.pose) && !
terrain_model->getHeight(feet.left.pose.position.x, feet.left.pose.position.y, feet.left.pose.position.z))
138 status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FeetPoseGenerator",
"updateFeetPose: Couldn't determine either 3D nor height of left foot!",
false);
140 if (!
terrain_model->update3DData(feet.right.pose) && !
terrain_model->getHeight(feet.right.pose.position.x, feet.right.pose.position.y, feet.right.pose.position.z))
141 status += ErrorStatusWarning(msgs::ErrorStatus::WARN_UNKNOWN,
"FeetPoseGenerator",
"updateFeetPose: Couldn't determine either 3D nor height of right foot!",
false);
154 feet.left.header.stamp = t.
stamp_;
155 feet.left.foot_index = msgs::Foot::LEFT;
159 feet.right.header.stamp = t.
stamp_;
160 feet.right.foot_index = msgs::Foot::RIGHT;
163 feet.header.frame_id = feet.left.header.frame_id = feet.right.header.frame_id = request_frame;
173 double yaw =
tf::getYaw(pose.pose.orientation);
177 feet.header = pose.header;
179 feet.left.header = pose.header;
180 feet.left.foot_index = msgs::Foot::LEFT;
181 feet.left.pose.position.x = pose.pose.position.x + shift_x;
182 feet.left.pose.position.y = pose.pose.position.y + shift_y;
183 feet.left.pose.position.z = pose.pose.position.z;
186 feet.right.header = pose.header;
187 feet.right.foot_index = msgs::Foot::RIGHT;
188 feet.right.pose.position.x = pose.pose.position.x - shift_x;
189 feet.right.pose.position.y = pose.pose.position.y - shift_y;
190 feet.right.pose.position.z = pose.pose.position.z;
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
static double getYaw(const Quaternion &bt_q)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
bool getParam(const std::string &key, std::string &s) const