Skip to content

Commit 4d83fbf

Browse files
bostoncleekbostoncleek
committed
developing impl using action client
1 parent 6974d64 commit 4d83fbf

File tree

3 files changed

+174
-44
lines changed

3 files changed

+174
-44
lines changed

‎core/include/moveit/task_constructor/stages/gpd_generator.h‎

Lines changed: 173 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -34,59 +34,190 @@
3434
Desc: Grasp generator stage using deep learning based grasp synthesizers
3535
*/
3636

37-
#pragma once
37+
#pragma once
3838

39-
#include <moveit/task_constructor/stages/generate_pose.h>
39+
#include <moveit/task_constructor/stages/generate_pose.h>
40+
#include <moveit/task_constructor/stages/action_base.h>
41+
#include <moveit/task_constructor/storage.h>
42+
#include <moveit/planning_scene/planning_scene.h>
4043

41-
#include <memory>
44+
#include <Eigen/Geometry>
45+
#include <eigen_conversions/eigen_msg.h>
4246

43-
#include <moveit_task_constructor_msgs/GenerateDeepGraspPoseAction.h>
44-
#include <actionlib/client/simple_action_client.h>
45-
// #include <actionlib/action_definition.h>
47+
#include <memory>
4648

47-
namespace moveit {
48-
namespace task_constructor {
49-
namespace stages {
49+
#include <moveit_task_constructor_msgs/GenerateDeepGraspPoseAction.h>
5050

51-
template<class ActionSpec>
52-
class ActionStageBase
53-
{
54-
private:
55-
ACTION_DEFINITION(ActionSpec);
5651

57-
public:
58-
ActionStageBase()
59-
{
60-
clientPtr_.reset(new actionlib::SimpleActionClient<ActionSpec>("topic_name", true));
61-
}
62-
// void feedbackCallback(const FeedbackConstPtr &feedback);
52+
namespace moveit {
53+
namespace task_constructor {
54+
namespace stages {
6355

64-
protected:
65-
std::unique_ptr<actionlib::SimpleActionClient<ActionSpec>> clientPtr_;
66-
};
6756

57+
template<class ActionSpec>
58+
class DeepGraspPose : public GeneratePose, ActionBase<ActionSpec>
59+
{
60+
private:
61+
typedef ActionBase<ActionSpec> ActionBaseT;
62+
ACTION_DEFINITION(ActionSpec);
63+
64+
public:
65+
DeepGraspPose(const std::string& action_name,
66+
const std::string& stage_name = "generate grasp pose");
67+
68+
void activeCallback() override;
6869

69-
class DeepGraspPose : public GeneratePose
70-
{
71-
public:
72-
DeepGraspPose(const std::string& name = "generate grasp pose");
70+
void feedbackCallback(const FeedbackConstPtr &feedback) override;
7371

74-
void init(const core::RobotModelConstPtr& robot_model) override;
75-
void compute() override;
72+
void doneCallback(const actionlib::SimpleClientGoalState& state,
73+
const ResultConstPtr &result) override;
7674

77-
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
78-
void setObject(const std::string& object) { setProperty("object", object); }
75+
void init(const core::RobotModelConstPtr& robot_model) override;
76+
void compute() override;
7977

80-
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
81-
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }
82-
void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
83-
void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); }
78+
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
79+
void setObject(const std::string& object) { setProperty("object", object); }
8480

85-
protected:
86-
void onNewSolution(const SolutionBase& s) override;
81+
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
82+
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }
83+
void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
84+
void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); }
85+
86+
protected:
87+
void onNewSolution(const SolutionBase& s) override;
88+
89+
private:
90+
bool found_candidates_;
91+
std::vector<geometry_msgs::PoseStamped> grasp_candidates_;
92+
std::vector<double> costs_;
93+
};
8794

88-
actionlib::SimpleActionClient<moveit_task_constructor_msgs::GenerateDeepGraspPoseAction> client_;
89-
};
90-
} // namespace stages
91-
} // namespace task_constructor
92-
} // namespace moveit
95+
template<class ActionSpec>
96+
DeepGraspPose<ActionSpec>::DeepGraspPose(const std::string& action_name,
97+
const std::string& stage_name)
98+
: GeneratePose(stage_name), ActionBaseT(action_name), found_candidates_(false) {
99+
auto& p = properties();
100+
p.declare<std::string>("eef", "name of end-effector");
101+
p.declare<std::string>("object");
102+
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
103+
104+
p.declare<boost::any>("pregrasp", "pregrasp posture");
105+
p.declare<boost::any>("grasp", "grasp posture");
106+
107+
ROS_INFO("Waiting for connection to grasp generation action server...");
108+
ActionBaseT::clientPtr_->waitForServer(ros::Duration(ActionBaseT::server_timeout_));
109+
}
110+
111+
112+
template<class ActionSpec>
113+
void DeepGraspPose<ActionSpec>::activeCallback(){
114+
ROS_INFO("Generate grasp goal now active ");
115+
// found_candidates_ = false;
116+
}
117+
118+
119+
template<class ActionSpec>
120+
void DeepGraspPose<ActionSpec>::feedbackCallback(const FeedbackConstPtr &feedback){
121+
// TODO: Check if costs are positive ?
122+
123+
// each candidate should have a cost
124+
if (feedback->grasp_candidates.size() != feedback->costs.size()){
125+
ROS_ERROR("Invalid input: each grasp candidate needs an associated cost");
126+
}
127+
else{
128+
grasp_candidates_.resize(feedback->grasp_candidates.size());
129+
costs_.resize(feedback->costs.size());
130+
131+
grasp_candidates_ = feedback->grasp_candidates;
132+
costs_ = feedback->costs;
133+
}
134+
}
135+
136+
137+
template<class ActionSpec>
138+
void DeepGraspPose<ActionSpec>::doneCallback(const actionlib::SimpleClientGoalState& state,
139+
const ResultConstPtr &result){
140+
// TODO: add completion timeout
141+
142+
if (state == actionlib::SimpleClientGoalState::SUCCEEDED){
143+
found_candidates_ = true;
144+
ROS_INFO("Successfully found grasp candidates (result): %s", result->grasp_state.c_str());
145+
}
146+
else{
147+
ROS_WARN("No grasp candidates found (state): %s", ActionBaseT::clientPtr_->getState().toString().c_str());
148+
}
149+
}
150+
151+
152+
template<class ActionSpec>
153+
void DeepGraspPose<ActionSpec>::init(const core::RobotModelConstPtr& robot_model){
154+
InitStageException errors;
155+
try {
156+
GeneratePose::init(robot_model);
157+
} catch (InitStageException& e) {
158+
errors.append(e);
159+
}
160+
161+
const auto& props = properties();
162+
163+
// check angle_delta
164+
if (props.get<double>("angle_delta") == 0.){
165+
errors.push_back(*this, "angle_delta must be non-zero");
166+
}
167+
168+
// check availability of object
169+
props.get<std::string>("object");
170+
// check availability of eef
171+
const std::string& eef = props.get<std::string>("eef");
172+
if (!robot_model->hasEndEffector(eef)){
173+
errors.push_back(*this, "unknown end effector: " + eef);
174+
}
175+
else {
176+
// check availability of eef pose
177+
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
178+
const std::string& name = props.get<std::string>("pregrasp");
179+
std::map<std::string, double> m;
180+
if (!jmg->getVariableDefaultPositions(name, m)){
181+
errors.push_back(*this, "unknown end effector pose: " + name);
182+
}
183+
}
184+
185+
if (errors){
186+
throw errors;
187+
}
188+
}
189+
190+
191+
template<class ActionSpec>
192+
void DeepGraspPose<ActionSpec>::compute(){
193+
194+
195+
}
196+
197+
198+
template<class ActionSpec>
199+
void DeepGraspPose<ActionSpec>::onNewSolution(const SolutionBase& s){
200+
planning_scene::PlanningSceneConstPtr scene = s.end()->scene();
201+
202+
const auto& props = properties();
203+
const std::string& object = props.get<std::string>("object");
204+
if (!scene->knowsFrameTransform(object)) {
205+
const std::string msg = "object '" + object + "' not in scene";
206+
if (storeFailures()) {
207+
InterfaceState state(scene);
208+
SubTrajectory solution;
209+
solution.markAsFailure();
210+
solution.setComment(msg);
211+
spawn(std::move(state), std::move(solution));
212+
} else{
213+
ROS_WARN_STREAM_NAMED("GenerateGraspPose", msg);
214+
}
215+
return;
216+
}
217+
218+
upstream_solutions_.push(&s);
219+
}
220+
221+
} // namespace stages
222+
} // namespace task_constructor
223+
} // namespace moveit

‎core/src/CMakeLists.txt‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
11
add_library(${PROJECT_NAME}
2-
${PROJECT_INCLUDE}/action_base.h
32
${PROJECT_INCLUDE}/container.h
43
${PROJECT_INCLUDE}/container_p.h
54
${PROJECT_INCLUDE}/cost_queue.h

‎core/src/stages/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ add_library(${PROJECT_NAME}_stages
22
${PROJECT_INCLUDE}/stages/modify_planning_scene.h
33
${PROJECT_INCLUDE}/stages/fix_collision_objects.h
44

5+
${PROJECT_INCLUDE}/stages/action_base.h
56
${PROJECT_INCLUDE}/stages/current_state.h
67
${PROJECT_INCLUDE}/stages/gpd_generator.h
78
${PROJECT_INCLUDE}/stages/fixed_state.h
@@ -28,7 +29,6 @@ add_library(${PROJECT_NAME}_stages
2829
generate_pose.cpp
2930
generate_grasp_pose.cpp
3031
generate_place_pose.cpp
31-
gpd_generator.cpp
3232
compute_ik.cpp
3333
predicate_filter.cpp
3434

0 commit comments

Comments
 (0)