def __init__(self,
analyze_grasp_topic="/moveit_trajectory_planner/check_reachability",
demo_pose_topic="/graspit/analyze_demo_pose", move_group_name='StaubliArm',
grasp_approach_tran_frame='/approach_tran'):
"""
:type analyze_grasp_topic: string
:param analyze_grasp_topic: topic to subscribe to for analyze grasp requests
:type demo_pose_topic: string
:param demo_pose_topic: topic to set service for responding to demonstration pose analysis requests
:type move_group_name: string
:param move_group_name
:type grasp_approach_tran_frame: string
:param grasp_approach_tran_frame: The frame of of the pose of the approach direction of the grasp.
"""
self.analyze_grasp_service = rospy.Service(analyze_grasp_topic,
graspit_msgs.srv.LocationInfo,
self.analyze_grasp_reachability)
self.analyze_pose_service = rospy.Service(demo_pose_topic,
graspit_msgs.srv.AnalyzePose,
self.analyze_demonstration_pose)
self._analyze_grasp_as = actionlib.SimpleActionServer("analyze_grasp_action",
graspit_msgs.msg.CheckGraspReachabilityAction,
execute_cb=self.analyze_grasp_reachability_cb,
auto_start=False)
self._analyze_grasp_as.start()
self.demonstration_pose_analyzer = DemonstrationPoseAnalyzer()
moveit_commander.roscpp_initialize(sys.argv)
group = moveit_commander.MoveGroupCommander(move_group_name)
self.grasp_reachability_analyzer = GraspReachabilityAnalyzer(group, grasp_approach_tran_frame)
self.grasp_reachability_analyzer.planner_id = move_group_name + rospy.get_param('grasp_analyzer/planner_config_name', '[PRMkConfigDefault]')
rospy.loginfo(self.__class__.__name__ + " is inited")
class GraspAnalyzerNode(object):
def __init__(self,
analyze_grasp_topic="/moveit_trajectory_planner/check_reachability",
demo_pose_topic="/graspit/analyze_demo_pose", move_group_name='StaubliArm',
grasp_approach_tran_frame='/approach_tran'):
"""
:type analyze_grasp_topic: string
:param analyze_grasp_topic: topic to subscribe to for analyze grasp requests
:type demo_pose_topic: string
:param demo_pose_topic: topic to set service for responding to demonstration pose analysis requests
:type move_group_name: string
:param move_group_name
:type grasp_approach_tran_frame: string
:param grasp_approach_tran_frame: The frame of of the pose of the approach direction of the grasp.
"""
self.analyze_grasp_service = rospy.Service(analyze_grasp_topic,
graspit_msgs.srv.LocationInfo,
self.analyze_grasp_reachability)
self.analyze_pose_service = rospy.Service(demo_pose_topic,
graspit_msgs.srv.AnalyzePose,
self.analyze_demonstration_pose)
self._analyze_grasp_as = actionlib.SimpleActionServer("analyze_grasp_action",
graspit_msgs.msg.CheckGraspReachabilityAction,
execute_cb=self.analyze_grasp_reachability_cb,
auto_start=False)
self._analyze_grasp_as.start()
self.demonstration_pose_analyzer = DemonstrationPoseAnalyzer()
moveit_commander.roscpp_initialize(sys.argv)
group = moveit_commander.MoveGroupCommander(move_group_name)
self.grasp_reachability_analyzer = GraspReachabilityAnalyzer(group, grasp_approach_tran_frame)
self.grasp_reachability_analyzer.planner_id = move_group_name + rospy.get_param('grasp_analyzer/planner_config_name', '[PRMkConfigDefault]')
rospy.loginfo(self.__class__.__name__ + " is inited")
def analyze_demonstration_pose(self, demo_pose):
"""
:type demo_pose: graspit_msgs.srv.AnalyzePoseRequest
:rtype response: graspit_msgs.srv.AnalyzePoseResponse
"""
response = graspit_msgs.srv.AnalyzePoseResponse()
success_probability = self.demonstration_pose_analyzer.analyze_pose(demo_pose)
response.success_probability = success_probability
return response
def analyze_grasp_reachability(self, location_info_req):
"""
@param location_info_req: grasp message to analyze
:type location_info_req: moveit_msgs.srv.LocationInfoRequest
@return: Whether the grasp is expected to succeed
@rtype: bool
"""
rospy.loginfo(self.__class__.__name__ + " received analyze grasp request: " + str(location_info_req))
grasp_msg = location_info_req.grasp
success, result = self.grasp_reachability_analyzer.query_moveit_for_reachability(grasp_msg)
self.demonstration_pose_analyzer.train_model(grasp_msg, success)
response = graspit_msgs.srv.LocationInfoResponse(success)
rospy.loginfo(self.__class__.__name__ + " finished analyze grasp request: " + str(response))
return response
def analyze_grasp_reachability_cb(self, goal):
"""
@param location_info_req: grasp message to analyze
:type location_info_req: moveit_msgs.srv.LocationInfoRequest
@return: Whether the grasp is expected to succeed
@rtype: bool
"""
_result = graspit_msgs.msg.CheckGraspReachabilityResult()
#rospy.loginfo(self.__class__.__name__ + " received analyze grasp request: " + str(goal))
grasp_msg = goal.grasp
success, result = self.grasp_reachability_analyzer.query_moveit_for_reachability(grasp_msg)
self.demonstration_pose_analyzer.train_model(grasp_msg, success)
_result.isPossible = success
_result.grasp_id = goal.grasp.grasp_id
rospy.loginfo(self.__class__.__name__ + " finished analyze grasp request: " + str(_result))
self._analyze_grasp_as.set_succeeded(_result)
return _result
