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
