@@ -51,6 +51,9 @@ namespace moveit {
5151namespace task_constructor {
5252namespace stages {
5353
54+ constexpr char LOGNAME[] = " deep_grasp_generator" ;
55+
56+
5457/* *
5558* @brief Generate grasp candidates using deep learning approaches
5659* @param ActionSpec - action message (action message name + "ACTION")
@@ -60,8 +63,6 @@ template<class ActionSpec>
6063class DeepGraspPose : public GeneratePose , ActionBase<ActionSpec>
6164{
6265
63- const std::string LOGNAME = " deep_grasp_generator" ;
64-
6566private:
6667 typedef ActionBase<ActionSpec> ActionBaseT;
6768 ACTION_DEFINITION (ActionSpec);
@@ -128,12 +129,12 @@ DeepGraspPose<ActionSpec>::DeepGraspPose(const std::string& action_name,
128129 found_candidates_(false ){
129130
130131 auto & p = properties ();
131- p.declare <std::string>(" eef" , " name of end-effector" );
132- p.declare <std::string>(" object" );
133- p.declare <double >(" angle_delta" , 0.1 , " angular steps (rad)" );
132+ p.template declare <std::string>(" eef" , " name of end-effector" );
133+ p.template declare <std::string>(" object" );
134+ p.template declare <double >(" angle_delta" , 0.1 , " angular steps (rad)" );
134135
135- p.declare <boost::any>(" pregrasp" , " pregrasp posture" );
136- p.declare <boost::any>(" grasp" , " grasp posture" );
136+ p.template declare <boost::any>(" pregrasp" , " pregrasp posture" );
137+ p.template declare <boost::any>(" grasp" , " grasp posture" );
137138
138139 ROS_INFO_NAMED (LOGNAME, " Waiting for connection to grasp generation action server..." );
139140 ActionBaseT::clientPtr_->waitForServer (ros::Duration (ActionBaseT::server_timeout_));
@@ -235,21 +236,21 @@ void DeepGraspPose<ActionSpec>::init(const core::RobotModelConstPtr& robot_model
235236 const auto & props = properties ();
236237
237238 // check angle_delta
238- if (props.get <double >(" angle_delta" ) == 0 .){
239+ if (props.template get <double >(" angle_delta" ) == 0 .){
239240 errors.push_back (*this , " angle_delta must be non-zero" );
240241 }
241242
242243 // check availability of object
243- props.get <std::string>(" object" );
244+ props.template get <std::string>(" object" );
244245 // check availability of eef
245- const std::string& eef = props.get <std::string>(" eef" );
246+ const std::string& eef = props.template get <std::string>(" eef" );
246247 if (!robot_model->hasEndEffector (eef)){
247248 errors.push_back (*this , " unknown end effector: " + eef);
248249 }
249250 else {
250251 // check availability of eef pose
251252 const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector (eef);
252- const std::string& name = props.get <std::string>(" pregrasp" );
253+ const std::string& name = props.template get <std::string>(" pregrasp" );
253254 std::map<std::string, double > m;
254255 if (!jmg->getVariableDefaultPositions (name, m)){
255256 errors.push_back (*this , " unknown end effector pose: " + name);
@@ -272,11 +273,11 @@ void DeepGraspPose<ActionSpec>::compute(){
272273
273274 // set end effector pose
274275 const auto & props = properties ();
275- const std::string& eef = props.get <std::string>(" eef" );
276+ const std::string& eef = props.template get <std::string>(" eef" );
276277 const moveit::core::JointModelGroup* jmg = scene->getRobotModel ()->getEndEffector (eef);
277278
278279 robot_state::RobotState& robot_state = scene->getCurrentStateNonConst ();
279- robot_state.setToDefaultValues (jmg, props.get <std::string>(" pregrasp" ));
280+ robot_state.setToDefaultValues (jmg, props.template get <std::string>(" pregrasp" ));
280281
281282
282283 // compose/send goal
@@ -309,7 +310,7 @@ void DeepGraspPose<ActionSpec>::onNewSolution(const SolutionBase& s){
309310 planning_scene::PlanningSceneConstPtr scene = s.end ()->scene ();
310311
311312 const auto & props = properties ();
312- const std::string& object = props.get <std::string>(" object" );
313+ const std::string& object = props.template get <std::string>(" object" );
313314 if (!scene->knowsFrameTransform (object)) {
314315 const std::string msg = " object '" + object + " ' not in scene" ;
315316 if (storeFailures ()) {
0 commit comments