def _result_cb(userdata, status, result):
""" This function will analize the status of the response
If the status is aborted, will call the action again with the same
orientation, but with the distance -0,5. If the status now is succeeded,
then call the move_by with x=0.5 in front.
:type status: actionlib.GoalStatus
:type result: MoveBaseResult
"""
if status != GoalStatus.SUCCEEDED:
rospy.loginfo(C.BACKGROUND_YELLOW + "Retrying go to the target goal" + C.NATIVE_COLOR)
new_pose = pose_at_distance(self.nav_goal.target_pose.pose, DISTANCE_TO_RETRY )
self.nav_goal.target_pose.pose = new_pose
self.nav_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(move_base_action_name, MoveBaseAction, goal=self.nav_goal)
result_status = move_action.execute(userdata)
if result_status == succeeded:
new_goal = MoveBaseGoal()
new_goal.target_pose.header.frame_id = FRAME_BASE_LINK
new_goal.target_pose.pose.position.x = DISTANCE_TO_RETRY
new_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(MOVE_BY_ACTION_NAME , MoveBaseAction, goal=new_goal)
return move_action.execute(userdata)
def execute(self, userdata):
self._goal.planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "SchunkArm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
motion_plan_request.planner_id = ""
self._goal.motion_plan_request = motion_plan_request
desired_pose = SimplePoseConstraint()
desired_pose.header.frame_id = userdata.parent_frame
desired_pose.link_name = "Gripper"
desired_pose.pose.position.x = userdata.x
desired_pose.pose.position.y = userdata.y
desired_pose.pose.position.z = userdata.z
quat = tf.transformations.quaternion_from_euler(-math.pi/2, math.pi/2, userdata.phi)
desired_pose.pose.orientation = Quaternion(*quat)
desired_pose.absolute_position_tolerance.x = 0.02
desired_pose.absolute_position_tolerance.y = 0.02
desired_pose.absolute_position_tolerance.z = 0.02
desired_pose.absolute_roll_tolerance = 0.04
desired_pose.absolute_pitch_tolerance = 0.04
desired_pose.absolute_yaw_tolerance = 0.04
add_goal_constraint_to_move_arm_goal(desired_pose, self._goal)
srv_out = SimpleActionState.execute(self, userdata)
return srv_out
def execute(self, userdata):
self._goal.planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "SchunkArm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
motion_plan_request.planner_id = ""
self._goal.motion_plan_request = motion_plan_request
desired_pose = SimplePoseConstraint()
desired_pose.header.frame_id = userdata.parent_frame
desired_pose.link_name = "Gripper"
desired_pose.pose.position.x = userdata.x
desired_pose.pose.position.y = userdata.y
desired_pose.pose.position.z = userdata.z
quat = tf.transformations.quaternion_from_euler(
-math.pi / 2, math.pi / 2, userdata.phi)
desired_pose.pose.orientation = Quaternion(*quat)
desired_pose.absolute_position_tolerance.x = 0.02
desired_pose.absolute_position_tolerance.y = 0.02
desired_pose.absolute_position_tolerance.z = 0.02
desired_pose.absolute_roll_tolerance = 0.04
desired_pose.absolute_pitch_tolerance = 0.04
desired_pose.absolute_yaw_tolerance = 0.04
add_goal_constraint_to_move_arm_goal(desired_pose, self._goal)
srv_out = SimpleActionState.execute(self, userdata)
return srv_out
def arms_out_of_self_colision(self):
self._print_title("ARMS OUT OF SELF COLISION")
if self.using_the_robot:
if self.__check_action(ARMS_ACTION_NAME) == aborted:
return aborted
else:
self._print_warning("Not checking. ROS_MASTER_URI nor COMPUTER_NAME contains %s" % ROBOTS_NAME)
return aborted
robot = os.environ.get('PAL_ROBOT')
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
rospy.loginfo('remotelly_executing: %s' % remotelly_executing) # FIXME: Remove this line after test in the robot
MOTION_FOLDER_PATH = ''
plan = False
checkSafety = False
if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3' or remotelly_executing == -1:
check_for_door = True # FIXME: This variable is not being used.
MOTION_FOLDER_PATH = "/mnt_flash/stacks/reem_alive/alive_engine/config/database/Stopped/interact_1.xml"
if remotelly_executing == -1:
MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml'
plan = True
checkSafety = False
motion_goal = MotionManagerGoal()
motion_goal.plan = plan
motion_goal.filename = MOTION_FOLDER_PATH
motion_goal.checkSafety = checkSafety
motion_goal.repeat = False
motion_manager_action = SimpleActionState(ARMS_ACTION_NAME, MotionManagerAction, goal=motion_goal)
userdata_hacked = {}
status = motion_manager_action.execute(userdata_hacked)
if status == succeeded:
rospy.loginfo(self.colors.GREEN_BOLD + "Arms out of self colisin: OK " + self.colors.NATIVE_COLOR)
else:
self.ALL_OK = False
rospy.loginfo(self.colors.RED_BOLD + "Arms out of self colisin: Failed " + self.colors.NATIVE_COLOR)
