Go to the documentation of this file.00001
00014
00015 #include "graspdb/Grasp.h"
00016
00017 using namespace std;
00018 using namespace rail::pick_and_place::graspdb;
00019
00020 Grasp::Grasp(const uint32_t id, const uint32_t grasp_model_id, const Pose &grasp_pose, const string &eef_frame_id,
00021 const uint32_t successes, const uint32_t attempts, const time_t created)
00022 : Entity(id, created),
00023 grasp_pose_(grasp_pose), eef_frame_id_(eef_frame_id)
00024 {
00025 grasp_model_id_ = grasp_model_id;
00026 successes_ = successes;
00027 attempts_ = attempts;
00028 }
00029
00030 Grasp::Grasp(const Pose &grasp_pose, const uint32_t grasp_model_id, const string &eef_frame_id,
00031 const uint32_t successes, const uint32_t attempts)
00032 : grasp_pose_(grasp_pose), eef_frame_id_(eef_frame_id)
00033 {
00034 grasp_model_id_ = grasp_model_id;
00035 successes_ = successes;
00036 attempts_ = attempts;
00037 }
00038
00039 Grasp::Grasp(const rail_pick_and_place_msgs::GraspWithSuccessRate &g, const uint32_t grasp_model_id)
00040 : Entity(g.id, g.created.sec),
00041 grasp_pose_(g.grasp.grasp_pose), eef_frame_id_(g.grasp.eef_frame_id)
00042 {
00043 grasp_model_id_ = grasp_model_id;
00044 successes_ = 0;
00045 attempts_ = 0;
00046 }
00047
00048 uint32_t Grasp::getGraspModelID() const
00049 {
00050 return grasp_model_id_;
00051 }
00052
00053 void Grasp::setGraspModelID(const uint32_t grasp_model_id)
00054 {
00055 grasp_model_id_ = grasp_model_id;
00056 }
00057
00058 const Pose &Grasp::getGraspPose() const
00059 {
00060 return grasp_pose_;
00061 }
00062
00063 Pose &Grasp::getGraspPose()
00064 {
00065 return grasp_pose_;
00066 }
00067
00068 void Grasp::setGraspPose(const Pose &grasp_pose)
00069 {
00070 grasp_pose_ = grasp_pose;
00071 }
00072
00073 const string &Grasp::getEefFrameID() const
00074 {
00075 return eef_frame_id_;
00076 }
00077
00078 void Grasp::setEefFrameID(const string &eef_frame_id)
00079 {
00080 eef_frame_id_ = eef_frame_id;
00081 }
00082
00083 uint32_t Grasp::getSuccesses() const
00084 {
00085 return successes_;
00086 }
00087
00088 void Grasp::setSuccesses(const uint32_t successes)
00089 {
00090 successes_ = successes;
00091 }
00092
00093 uint32_t Grasp::getAttempts() const
00094 {
00095 return attempts_;
00096 }
00097
00098 void Grasp::setAttempts(const uint32_t attempts)
00099 {
00100 attempts_ = attempts;
00101 }
00102
00103 double Grasp::getSuccessRate() const
00104 {
00105
00106 return (attempts_ == 0) ? 0 : ((double) successes_) / ((double) attempts_);
00107 }
00108
00109
00110 rail_pick_and_place_msgs::GraspWithSuccessRate Grasp::toROSGraspWithSuccessRateMessage() const
00111 {
00112 rail_pick_and_place_msgs::GraspWithSuccessRate g;
00113 g.id = this->getID();
00114 g.grasp.grasp_pose = grasp_pose_.toROSPoseStampedMessage();
00115 g.grasp.eef_frame_id = eef_frame_id_;
00116 g.grasp.successes = successes_;
00117 g.grasp.attempts = attempts_;
00118 g.created.nsec = 0;
00119 g.created.sec = this->getCreated();
00120 return g;
00121 }