class GraspAnalyzerNode(object): def __init__(self): rospy.init_node('grasp_analyzer_node') analyze_grasp_topic = "analyze_grasp_action" move_group_name = rospy.get_param('/arm_name', 'manipulator') grasp_approach_tran_frame = rospy.get_param('approach_tran_frame', '/approach_tran') planner_id = move_group_name + rospy.get_param('grasp_analyzer/planner_config_name', '[PRMkConfigDefault]') moveit_commander.roscpp_initialize(sys.argv) group = moveit_commander.MoveGroupCommander(move_group_name) self.grasp_reachability_analyzer = GraspReachabilityAnalyzer(group, grasp_approach_tran_frame, planner_id) self._analyze_grasp_as = actionlib.SimpleActionServer(analyze_grasp_topic, graspit_msgs.msg.CheckGraspReachabilityAction, execute_cb=self.analyze_grasp_reachability_cb, auto_start=False) self._analyze_grasp_as.start() rospy.loginfo(self.__class__.__name__ + " is inited") def analyze_grasp_reachability_cb(self, goal): """ @return: Whether the grasp is expected to succeed @rtype: bool """ success, result = self.grasp_reachability_analyzer.query_moveit_for_reachability(goal.grasp) _result = graspit_msgs.msg.CheckGraspReachabilityResult() _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
def __init__(self, node_name='grasp_execution_node', manual_mode=False): rospy.init_node(node_name) self.use_robot_hw = rospy.get_param('use_robot_hw', False) self.grasp_approach_tran_frame = rospy.get_param('/approach_tran_frame', '/approach_tran') self.trajectory_display_topic = rospy.get_param('trajectory_display_topic', "/move_group/display_planned_path") self.grasp_listener_topic = rospy.get_param('grasp_listener_topic', "/graspit/grasps") self.move_group_name = rospy.get_param('/arm_name', 'StaubliArm') self.reachability_planner_id = self.move_group_name + rospy.get_param('grasp_executer/planner_config_name', 'SBLkConfigDefault2') self.graspit_status_publisher = rospy.Publisher("/graspit/status", graspit_msgs.msg.GraspStatus) self.trajectory_action_client_topic = rospy.get_param('trajectory_action_name', '/setFollowTrajectory') if not manual_mode: self.grasp_listener = rospy.Subscriber(self.grasp_listener_topic, graspit_msgs.msg.Grasp, callback=self.handle_grasp_msg, queue_size=1) display_trajectory_publisher = rospy.Publisher(self.trajectory_display_topic, moveit_msgs.msg.DisplayTrajectory) hand_manager = GraspManager.GraspManager(importlib.import_module(rospy.get_param('hand_manager')), self.move_group_name) trajectory_action_client = actionlib.SimpleActionClient(self.trajectory_action_client_topic, control_msgs.msg.FollowJointTrajectoryAction) moveit_commander.roscpp_initialize(sys.argv) group = moveit_commander.MoveGroupCommander(self.move_group_name) grasp_reachability_analyzer = GraspReachabilityAnalyzer(group, self.grasp_approach_tran_frame) grasp_reachability_analyzer.planner_id = self.reachability_planner_id self.robot_interface = robot_interface_lib.RobotInterface(trajectory_action_client=trajectory_action_client, display_trajectory_publisher=display_trajectory_publisher, hand_manager=hand_manager, group=group, grasp_reachability_analyzer=grasp_reachability_analyzer) self.execution_pipeline = execution_pipeline.GraspExecutionPipeline(self.robot_interface, rospy.get_param('execution_stages', ['MoveToPreGraspPosition', 'PreshapeHand', 'Approach', 'CloseHand', 'Lift'])) self.last_grasp_time = 0 rospy.loginfo(self.__class__.__name__ + " is initialized")
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 __init__(self): rospy.init_node('grasp_analyzer_node') analyze_grasp_topic = "analyze_grasp_action" move_group_name = rospy.get_param('/arm_name', 'manipulator') grasp_approach_tran_frame = rospy.get_param('approach_tran_frame', '/approach_tran') planner_id = move_group_name + rospy.get_param('grasp_analyzer/planner_config_name', '[PRMkConfigDefault]') moveit_commander.roscpp_initialize(sys.argv) group = moveit_commander.MoveGroupCommander(move_group_name) self.grasp_reachability_analyzer = GraspReachabilityAnalyzer(group, grasp_approach_tran_frame, planner_id) self._analyze_grasp_as = actionlib.SimpleActionServer(analyze_grasp_topic, graspit_msgs.msg.CheckGraspReachabilityAction, execute_cb=self.analyze_grasp_reachability_cb, auto_start=False) self._analyze_grasp_as.start() rospy.loginfo(self.__class__.__name__ + " is inited")
def __init__(self, node_name='grasp_execution_node', manual_mode=False): rospy.init_node(node_name) self.use_robot_hw = rospy.get_param('use_robot_hw', False) self.grasp_approach_tran_frame = rospy.get_param( '/approach_tran_frame', '/approach_tran') self.trajectory_display_topic = rospy.get_param( 'trajectory_display_topic', "/move_group/display_planned_path") self.grasp_listener_topic = rospy.get_param('grasp_listener_topic', "/graspit/grasps") self.move_group_name = rospy.get_param('/arm_name', 'StaubliArm') self.reachability_planner_id = self.move_group_name + rospy.get_param( 'grasp_executer/planner_config_name', 'SBLkConfigDefault2') self.graspit_status_publisher = rospy.Publisher( "/graspit/status", graspit_msgs.msg.GraspStatus) self.trajectory_action_client_topic = rospy.get_param( 'trajectory_action_name', '/setFollowTrajectory') if not manual_mode: self.grasp_listener = rospy.Subscriber( self.grasp_listener_topic, graspit_msgs.msg.Grasp, callback=self.handle_grasp_msg, queue_size=1) display_trajectory_publisher = rospy.Publisher( self.trajectory_display_topic, moveit_msgs.msg.DisplayTrajectory) hand_manager = GraspManager.GraspManager( importlib.import_module(rospy.get_param('hand_manager')), self.move_group_name) trajectory_action_client = actionlib.SimpleActionClient( self.trajectory_action_client_topic, control_msgs.msg.FollowJointTrajectoryAction) moveit_commander.roscpp_initialize(sys.argv) group = moveit_commander.MoveGroupCommander(self.move_group_name) grasp_reachability_analyzer = GraspReachabilityAnalyzer( group, self.grasp_approach_tran_frame) grasp_reachability_analyzer.planner_id = self.reachability_planner_id self.robot_interface = robot_interface_lib.RobotInterface( trajectory_action_client=trajectory_action_client, display_trajectory_publisher=display_trajectory_publisher, hand_manager=hand_manager, group=group, grasp_reachability_analyzer=grasp_reachability_analyzer) self.execution_pipeline = execution_pipeline.GraspExecutionPipeline( self.robot_interface, rospy.get_param('execution_stages', [ 'MoveToPreGraspPosition', 'PreshapeHand', 'Approach', 'CloseHand', 'Lift' ])) self.last_grasp_time = 0 rospy.loginfo(self.__class__.__name__ + " is initialized")
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