예제 #1
0
def move_forward():
    global current_pos
    global facing
    pub.publish(stdmsg.Empty())
    rospy.sleep(0.5)
    print "forward start x: {}".format(x_displacement)
    r = rospy.Rate(11)
    while (x_displacement < 0.47 and not rospy.is_shutdown()):
        #while (x_displacement < 0.2  and not rospy.is_shutdown()):
        command.linear.x = 0.1
        constant_command_service(command)
        r.sleep()

    command.linear.x = 0.0
    constant_command_service(command)
    print "forward end x: {}".format(x_displacement)
    pub.publish(stdmsg.Empty())
    rospy.sleep(0.5)

    if facing == LEFT:
        current_pos = (current_pos[0] - 1, current_pos[1])
    elif facing == RIGHT:
        current_pos = (current_pos[0] + 1, current_pos[1])
    elif facing == UP:
        current_pos = (current_pos[0], current_pos[1] - 1)
    elif facing == DOWN:
        current_pos = (current_pos[0], current_pos[1] + 1)
예제 #2
0
 def execute(self):
     self._dynamic_reconfigure_client.update_configuration(
         {'enabled': 'True'})
     found_markers = self._initialise_rotation()
     if not found_markers:
         result = self._rotate.execute()
         if not result or self._stop_requested:
             self._post_execute(False)
             return
     rospy.loginfo("AR Pair Approach : found both ar pair markers.")
     if self._parameters['search_only']:
         rospy.loginfo(
             "AR Pair Approach : aborting initialisation and approach as requested."
         )
         return
     rospy.loginfo(
         "AR Pair Approach : setting an initial pose from the global ar pair reference."
     )
     self._publishers['initial_pose_trigger'].publish(std_msgs.Empty())
     rospy.loginfo("AR Pair Approach : enabling the approach controller")
     self._publishers['enable_approach_controller'].publish(
         std_msgs.Empty())
     while not rospy.is_shutdown() and not self._stop_requested:
         if self._controller_finished:
             self._controller_finished = False
             break
         rospy.sleep(0.1)
     rospy.loginfo("AR Pair Approach : disabling the approach controller")
     self._publishers['disable_approach_controller'].publish(
         std_msgs.Empty())
     if rospy.is_shutdown() or self._stop_requested:
         self._post_execute(False)
     else:
         self._post_execute(True)
예제 #3
0
 def calibrate(self, timeout=5.0):
     """
     Calibrate the gripper
     """
     self._pub_calibrate.publish(stdmsg.Empty())
     self.enable()
     dataflow.wait_for(
         test=lambda: self.calibrated(),
         timeout=timeout,
         body=lambda: self._pub_calibrate.publish(stdmsg.Empty())
         )
예제 #4
0
def initialize_commands():
    rospy.init_node('mazesolvernode', anonymous=True)
    rospy.wait_for_service('make_maze')
    rospy.wait_for_service('print_maze')
    rospy.wait_for_service('get_wall')
    rospy.wait_for_service('constant_command')
    # odometry

    global pub
    pub = rospy.Publisher('/mobile_base/commands/reset_odometry',
                          stdmsg.Empty,
                          queue_size=10)
    rospy.Subscriber('/odom', Odometry, odometry_callback)
    while (pub.get_num_connections() <= 0):
        rospy.sleep(0.1)

    pub.publish(stdmsg.Empty())
    ## end odom

    global make_maze_service, print_maze_service, get_wall_service
    global constant_command_service
    make_maze_service = rospy.ServiceProxy('make_maze', MakeNewMaze)
    print_maze_service = rospy.ServiceProxy('print_maze', std_srv.Empty)
    get_wall_service = rospy.ServiceProxy('get_wall', GetMazeWall)
    constant_command_service = rospy.ServiceProxy('constant_command',
                                                  ConstantCommand)

    #calibrate_odometry()
    # runs code
    make_maze_service(MAZE_SIZE, MAZE_SIZE)
    print_maze_service()
    solve_maze()
예제 #5
0
def test_publish_with_existing_data():
    console.banner("Publish Existing Data")

    py_trees.blackboard.Blackboard.set(variable_name=blackboard_key(),
                                       value=std_msgs.Empty())

    subscriber, root = create_all_the_things()
    tree = py_trees_ros.trees.BehaviourTree(root=root,
                                            unicode_tree_debug=False)
    tree.setup()
    executor = rclpy.executors.SingleThreadedExecutor()
    executor.add_node(subscriber.node)
    executor.add_node(tree.node)

    assert_banner()

    start_time = time.monotonic()
    while (
        (time.monotonic() - start_time) < timeout()) and subscriber.count == 0:
        tree.tick()
        executor.spin_once(timeout_sec=0.05)

    assert_details("root.status", "SUCCESS", root.status)
    assert (root.status == py_trees.common.Status.SUCCESS)

    py_trees.blackboard.Blackboard.clear()
    tree.shutdown()
    subscriber.shutdown()
    executor.shutdown()
예제 #6
0
    def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        if not ( goal[idx]["primitive_action"] in ['test'] ):
            return None
        
        # beahviors
        root  = py_trees.composites.Sequence(name="TestMove")
        frame = rospy.get_param("arm_base_frame", '/ur_arm_base')
        blackboard = py_trees.blackboard.Blackboard()
        
        s_move1 = MoveJoint.MOVEJ(name="Move1", controller_ns=controller_ns,
                                  action_goal=blackboard.init_config)


        pose = Pose()
        pose.position.z=-0.1
        goal_dict = {'pose': pose,
                     'frame': frame}
        
        s_move2 = MovePose.MOVEPR(name="Move2", controller_ns=controller_ns,
                                  action_goal=goal_dict)

        pose = Pose()
        pose.position.z=0.1
        goal_dict = {'pose': pose,
                     'frame': frame}
        s_move3 = MovePose.MOVEPR(name="Move3", controller_ns=controller_ns,
                                  action_goal=goal_dict)


        pose = Pose()
        pose.position.z=-0.1
        goal_dict = {'pose': pose,
                     'frame': frame}
        
        s_move4 = MovePose.MOVEPR(name="Move4", controller_ns=controller_ns,
                                  action_goal=goal_dict)

        pose = Pose()
        pose.position.z=0.1
        goal_dict = {'pose': pose,
                     'frame': frame}
        
        s_move5 = MovePose.MOVEPR(name="Move5", controller_ns=controller_ns,
                                  action_goal=goal_dict)
        
        root.add_children([s_move1, s_move3, s_move2]) #, s_move4, s_move5])

        return root
 def suppress_left(self):
     msg = std_msgs.Empty()
     if self._left:
         if self._contact:
             self.left_contact_pub.publish(msg)
         if self._collisions:
             self.left_collisions_pub.publish(msg)
         if self._body:
             self.left_body_pub.publish(msg)
         if self._overwrench:
             self.left_overwrench_pub.publish(msg)
         if self._gravity:
             self.left_gravity_pub.publish(msg)
예제 #8
0
def turn_left():
    global facing

    pub.publish(stdmsg.Empty())
    rospy.sleep(0.5)

    rospy.loginfo("started left turn, theta: {}".format(theta_displacement))
    r = rospy.Rate(11)
    while (theta_displacement < 1.48 and not rospy.is_shutdown()):
        command.angular.z = 0.5
        constant_command_service(command)
        r.sleep()

    command.angular.z = 0.0
    constant_command_service(command)

    rospy.loginfo("ended left turn")
    rospy.loginfo("left turn theta: {}".format(theta_displacement))
    pub.publish(stdmsg.Empty())
    rospy.sleep(0.5)

    facing = getNextLeftDirection()
예제 #9
0
    def create_root(idx="1",
                    goal=std_msgs.Empty(),
                    controller_ns="",
                    **kwargs):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        return None
예제 #10
0
 def _tracked_poses_callback(self, msg):
     if self._initialise:
         # send pose to pose initialisation node
         msg.header.stamp -= rospy.Duration(0.2) # TODO: get latest common time
         self._pub_init_pose.publish(msg)
         empty_msg = std_msgs.Empty()
         if not self.param['simulation']:
             # disable the pose tracker
             params = { 'enabled' : 'False' }
             config = self._client.update_configuration(params)
         # send result
         self._pub_result.publish(empty_msg)
         self.loginfo("Initialisation done.")
         self._initialise = False
예제 #11
0
    def disable(self, interactions_loader):
        '''
        @param interactions_loader : used to unload interactions.
        @type rocon_interactions.RoleAppLoader
        '''
        success = False
        message = "unknown error"
        self._lock.acquire()
        self._shutdown_publisher.publish(std_msgs.Empty())
        try:
            if self.msg.interactions != '':
                # Can raise YamlResourceNotFoundException, MalformedInteractionsYaml
                interactions_loader.load(self.msg.interactions, namespace=self._namespace, load=False)
            if self.msg.parameters != '':
                namespace = concert_msgs.Strings.SERVICE_NAMESPACE + '/' + self.msg.name
                load_parameters_from_file(self.msg.parameters, namespace, self.msg.name, load=False)

            launcher_type = self.msg.launcher_type
            force_kill = False

            if launcher_type == concert_msgs.ServiceProfile.TYPE_CUSTOM:
                count = 0
                while not rospy.is_shutdown() and self._proc.poll() is None:
                    rospy.rostime.wallsleep(0.5)
                    if count == 2 * ServiceInstance.shutdown_timeout:
                        self._proc.terminate()
                    if count == 2 * ServiceInstance.kill_timeout:
                        self.loginfo("waited too long, force killing..")
                        self._proc.kill()
                        force_kill = True
                        break
                    count = count + 1
            elif launcher_type == concert_msgs.ServiceProfile.TYPE_ROSLAUNCH:
                rospy.loginfo("Service Manager : shutting down roslaunched concert service [%s]" % self.msg.name)
                count = 0
                # give it some time to naturally die first.
                while self._roslaunch.pm and not self._roslaunch.pm.done:
                    if count == 2 * ServiceInstance.shutdown_timeout:
                        self._roslaunch.shutdown()
                    rospy.rostime.wallsleep(0.5)
                    count = count + 1
            elif launcher_type == concert_msgs.ServiceProfile.TYPE_SHADOW:
                pass  # no processes to kill
            success = True
            message = "wouldn't die so the concert got violent (force killed)" if force_kill else "died a pleasant death (terminated naturally)"
        except (rocon_interactions.YamlResourceNotFoundException, rocon_interactions.MalformedInteractionsYaml) as e:
            success = False
            message = "error while disabling [%s][%s]" % (self.msg.name, str(e))
        self._lock.release()
        return success, message
 def _init_services(self):
     '''
       This initialises all the app manager services. It depends on whether we're initialising for standalone,
       or connected (pairing/concert) modes. This should not be activated multiply!
     '''
     if self._initialising_services:
         # We could use a lock to protect this, but since the only places we call this is in the
         # and in the spin(), then we just use a flag to protect.
         return False
     self._initialising_services = True
     if self._services:
         for service in self._services.values():
             service.shutdown()
         for publisher in self._publishers.values():
             publisher.unregister()
         self._services = {}
         self._publishers = {}
     self._service_names = {}
     self._publisher_names = {}
     base_name = self._gateway_name if self._gateway_name else self._param['robot_name']  # latter option is for standalone mode 
     for name in self._default_service_names:
         self._service_names[name] = '/' + base_name + '/' + name
     for name in self._default_publisher_names:
         self._publisher_names[name] = '/' + base_name + '/' + name
     self._application_namespace = base_name + '/' + RappManager.default_application_namespace  # ns to push apps into (see rapp.py)
     try:
         # Advertisable services - we advertise these by default advertisement rules for the app manager's gateway.
         self._services['platform_info'] = rospy.Service(self._service_names['platform_info'], rapp_manager_srvs.GetPlatformInfo, self._process_platform_info)
         self._services['list_apps'] = rospy.Service(self._service_names['list_apps'], rapp_manager_srvs.GetAppList, self._process_get_app_list)
         self._services['status'] = rospy.Service(self._service_names['status'], rapp_manager_srvs.Status, self._process_status)
         self._services['invite'] = rospy.Service(self._service_names['invite'], rapp_manager_srvs.Invite, self._process_invite)
         # Flippable services
         self._services['start_app'] = rospy.Service(self._service_names['start_app'], rapp_manager_srvs.StartApp, self._process_start_app)
         self._services['stop_app'] = rospy.Service(self._service_names['stop_app'], rapp_manager_srvs.StopApp, self._process_stop_app)
         # Latched publishers
         self._publishers['app_list'] = rospy.Publisher(self._publisher_names['app_list'], rapp_manager_msgs.AppList, latch=True)
         # Force an update on the gateway
         self._gateway_publishers['force_update'].publish(std_msgs.Empty())
     except Exception as unused_e:
         traceback.print_exc(file=sys.stdout)
         self._initialising_services = False
         return False
     self._publish_app_list()
     self._initialising_services = False
     return True
예제 #13
0
파일: rotate.py 프로젝트: baidu31/catkin_wp
 def execute(self):
     '''
       Drop this into threading.Thread or QThread for execution
     '''
     if self._running:
         rospy.logerr(
             "AR Pair Search: already executing a motion, ignoring the request"
         )
         return
     self._cmd_vel_publisher = rospy.Publisher(self._cmd_vel_topic,
                                               geometry_msgs.Twist,
                                               queue_size=10)
     self._stop = False
     self._running = True
     start = rospy.get_rostime()
     rospy.sleep(0.3)
     result = False
     twist = self._twist
     while not self._stop and not rospy.is_shutdown():
         update = self.yaw_direction * self.yaw_absolute_rate / 10.0
         if math.fabs(twist.angular.z) < self.yaw_absolute_rate:
             twist.angular.z = twist.angular.z + update
         else:
             # Make sure it is exact so the inequality in the while loop doesn't mess up next time around
             twist.angular.z = self.yaw_direction * self.yaw_absolute_rate
         now = rospy.get_rostime()
         rospy.logdebug("AR Pair Search: rotate: %s rad/s [%ss]" %
                        (twist.angular.z, str(now.secs - start.secs)))
         try:
             self._cmd_vel_publisher.publish(twist)
         except rospy.ROSException:  # shutdown
             break
         self._rate.sleep()
     if not rospy.is_shutdown():
         cmd = geometry_msgs.Twist()
         cmd.angular.z = 0.0
         self._cmd_vel_publisher.publish(cmd)
         self._initialise_pose_trigger.publish(std_msgs.Empty())
         result = True
     self._cmd_vel_publisher.unregister()
     self._cmd_vel_publisher = None
     self._running = False
     return result
예제 #14
0
 def _init_callback(self, msg):
     self.loginfo("Initialisation started.")
     # enable the pose tracker
     if self.param['simulation']:
         pose_msg = geometry_msgs.PoseWithCovarianceStamped()
         pose_msg.header.frame_id = "ar_global"
         pose_msg.header.stamp = rospy.Time.now() - rospy.Duration(0.2) # TODO: get latest common time
         pose_msg.pose.pose.position.x = 1.0
         pose_msg.pose.pose.position.y = 0.0
         pose_msg.pose.pose.position.z = 0.0
         quat = tf.transformations.quaternion_from_euler(0, 0, 3.1416)
         pose_msg.pose.pose.orientation = geometry_msgs.Quaternion(*quat)
         self._pub_init_pose.publish(pose_msg)
         # send success right away
         empty_msg = std_msgs.Empty()
         self._pub_result.publish(empty_msg)
         self.loginfo("Initialisation done.")
     else:
         params = { 'enabled' : 'True' }
         config = self._client.update_configuration(params)
         self._initialise = True
예제 #15
0
    def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        # beahviors
        root = py_trees.composites.Sequence(name="Handover")
        blackboard = py_trees.blackboard.Blackboard()

        if goal[idx]["primitive_action"] in ['handover']:
            obj = goal[idx]['object'].encode('ascii','ignore')
            destination = goal[idx]['destination'].encode('ascii','ignore')
        else:
            return None

        # TODO
        if destination=='na':
            destination = None
        
        # ----------------- Init Task ----------------        
        s_init = MoveJoint.MOVEJ(name="Init", controller_ns=controller_ns,
                                  action_goal=blackboard.init_config)

        # ----------------- Handover ---------------------
        s_move1 = MoveJoint.MOVEJ(name="Front", controller_ns=controller_ns,
                                  action_goal=[np.pi/2, -np.pi/2., np.pi/2., np.pi/2., -np.pi/2., 0])
        s_move2 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
                                   action_goal=blackboard.gripper_open_pos, check_contact=True)        

        wm_remove = WorldModel.REMOVE(name="Delete", action_goal={'obj_name': obj})


        task = py_trees.composites.Sequence(name="Handover")
        task.add_children([s_move1, s_move2, s_init, wm_remove])
        return task
예제 #16
0
    def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
        
        root = py_trees.composites.Sequence(name="Attach")
        blackboard = py_trees.blackboard.Blackboard()

        if goal[idx]["primitive_action"] in ['move']:
            obj         = goal[idx]['object'].encode('ascii','ignore')
            destination = goal[idx]['destination'].encode('ascii','ignore')
        else:
            return None

       
        if destination=='na':
            destination='plate'
            print("no plate, so selected plate as a destination")


        #s_init3 = MoveJoint.MOVEJ(name="Init", controller_ns=controller_ns, 
                                   # action_goal=blackboard.init_config)

        pose_est2 = WorldModel.POSE_ESTIMATOR(name="Plan"+idx,
                                              object_dict = {'target':obj,
                                                             'destination': destination})

        s_move21 = MovePose.MOVEPROOT(name="Top1",
                                      controller_ns=controller_ns,
                                      action_goal={'pose': "Plan"+idx+"/place_top_pose"})
        s_move22 = MovePose.MOVEP(name="Approach", controller_ns=controller_ns,
                                 action_goal={'pose': "Plan"+idx+"/place_pose"})
        s_move23 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
                                action_goal=blackboard.gripper_open_pos,
                                force=blackboard.gripper_open_force)        
        s_move24 = MovePose.MOVEP(name="Top", controller_ns=controller_ns,
                                 action_goal={'pose': "Plan"+idx+"/place_top_pose"})

        attach = py_trees.composites.Sequence(name="Attach")
        attach.add_children([pose_est2, s_move21, s_move22, s_move23, s_move24])

        return attach
예제 #17
0
    def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", rec_topic_list=None):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        if not ( goal[idx]["primitive_action"] in ['gripper_close', 'gripper_open'] ):
            return None
        
        # beahviors
        ## root = py_trees.composites.Sequence(name="Gripper")
        root = py_trees.composites.Parallel(name="Gripper",\
                                            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)

        action = goal[idx]['primitive_action'].encode('ascii','ignore')

        blackboard = py_trees.blackboard.Blackboard()
        if action.find('gripper_close')>=0:        
            s_move = Gripper.GOTO(name="Close", controller_ns=controller_ns,
                                action_goal=blackboard.gripper_close_pos,
                                force=blackboard.gripper_close_force)        
        else:
            s_move = Gripper.GOTO(name="Open", controller_ns=controller_ns,
                                action_goal=blackboard.gripper_open_pos,
                                force=blackboard.gripper_open_force)        
            
        if rec_topic_list is None:
            root.add_child(s_move)
        else:
            logger = Rosbag.ROSBAG(name="logger", topic_list=rec_topic_list)
            root.add_children([s_move, logger])

        return root
예제 #18
0
 def on_button_off_clicked(self):
     start_message = std_msgs.Empty()
     self._off_pub.publish(start_message)
     disarm_message = std_msgs.Bool(False)
     self._arm_bridge_pub.publish(disarm_message)
 def pause(self):
     self._land_publisher.publish(msg.Empty())
예제 #20
0
    def create_root(idx="1", goal=std_msgs.Empty(), controller_ns="", **kwargs):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        # beahviors
        root = py_trees.composites.Sequence(name="Move")
        blackboard = py_trees.blackboard.Blackboard()
        grasp_offset_z = 0.02
        
        if goal[idx]["primitive_action"] in ['slide']:
            obj         = goal[idx]['object'].encode('ascii','ignore')
            destination = goal[idx]['destination'].encode('ascii','ignore')
        else:
            return None

        # TODO
        if destination=='na':
            destination='place_tray'
            print "destination is not assigned, so selected place-tray as a destination"

        
        # ----------------- Move Task ----------------        
        s_init3 = MoveJoint.MOVEJ(name="Init", controller_ns=controller_ns,
                                  action_goal=blackboard.init_config)

        # ----------------- Pick ---------------------
        slide = py_trees.composites.Sequence(name="Slide")
        pose_est1 = WorldModel.POSE_ESTIMATOR(name="Plan"+idx,
                                              object_dict = {'target': obj})
        s_move10 = MovePose.MOVEPROOT(name="Top1",
                                      controller_ns=controller_ns,
                                      action_goal={'pose': "Plan"+idx+"/grasp_top_pose"})
        s_move11 = MovePose.MOVEP(name="Top2", controller_ns=controller_ns,
                                 action_goal={'pose': "Plan"+idx+"/grasp_top_pose"})
        s_move12 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
                                    action_goal=blackboard.gripper_open_pos,
                                    force=blackboard.gripper_open_force)        
        s_move13 = MovePose.MOVEP(name="Approach", controller_ns=controller_ns,
                                 action_goal={'pose': "Plan"+idx+"/grasp_pose"})
        # s_move14 = Gripper.GOTO(name="Close", controller_ns=controller_ns,
        #                        action_goal=200)        

        pose_est2 = WorldModel.POSE_ESTIMATOR(name="Plan"+idx,
                                              object_dict = {'target': obj,
                                                             'destination': destination})
                                                # en_close_pose=True)
        s_move22 = MovePose.MOVES(name="Approach", controller_ns=controller_ns,
                                 action_goal={'pose': "Plan"+idx+"/place_pose"})
        # s_move23 = Gripper.GOTO(name="Open", controller_ns=controller_ns,
        #                        action_goal=50)        
        s_move24 = MovePose.MOVEP(name="Top", controller_ns=controller_ns,
                                 action_goal={'pose': "Plan"+idx+"/place_top_pose"})
        
        slide.add_children([pose_est1, s_move10, s_move11, s_move13, \
                           pose_est2, s_move22, s_move24, s_init3])
        task = py_trees.composites.Sequence(name="Move")
        task.add_child(slide)
        return task
예제 #21
0
파일: jobs.py 프로젝트: jr-bor/py_trees_ros
    def create_root(goal=std_msgs.Empty()):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        # beahviors
        root = py_trees.composites.Selector(name="Scan or Die")
        die = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Uh Oh",
                                                              colour="red")
        ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
        undock = py_trees_ros.actions.ActionClient(
            name="UnDock",
            action_namespace="/dock",
            action_spec=py_trees_msgs.DockAction,
            action_goal=py_trees_msgs.DockGoal(False))
        scan_or_be_cancelled = py_trees.composites.Selector(
            "Scan or Be Cancelled")
        cancelling = py_trees.composites.Sequence("Cancelling?")
        is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
            name="Cancel?",
            variable_name='event_cancel_button',
            expected_value=True)
        move_home_after_cancel = py_trees_ros.actions.ActionClient(
            name="Move Home",
            action_namespace="/move_base",
            action_spec=move_base_msgs.MoveBaseAction,
            action_goal=move_base_msgs.MoveBaseGoal())
        move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
        move_base = py_trees_ros.actions.ActionClient(
            name="Move Out",
            action_namespace="/move_base",
            action_spec=move_base_msgs.MoveBaseAction,
            action_goal=move_base_msgs.MoveBaseGoal())
        scanning = py_trees.composites.Parallel(
            name="Scanning",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext(
            "Context Switch")
        scan_rotate = py_trees_ros.actions.ActionClient(
            name="Rotate",
            action_namespace="/rotate",
            action_spec=py_trees_msgs.RotateAction,
            action_goal=py_trees_msgs.RotateGoal())
        scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(
            name="Flash Blue", colour="blue")
        move_home_after_scan = py_trees_ros.actions.ActionClient(
            name="Move Home",
            action_namespace="/move_base",
            action_spec=move_base_msgs.MoveBaseAction,
            action_goal=move_base_msgs.MoveBaseGoal())
        celebrate = py_trees.composites.Parallel(
            name="Celebrate",
            policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(
            name="Flash Green", colour="green")
        celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
        dock = py_trees_ros.actions.ActionClient(
            name="Dock",
            action_namespace="/dock",
            action_spec=py_trees_msgs.DockAction,
            action_goal=py_trees_msgs.DockGoal(True))
        # Subtree
        root.add_children([ere_we_go, die])
        ere_we_go.add_children([undock, scan_or_be_cancelled, dock, celebrate])
        scan_or_be_cancelled.add_children([cancelling, move_out_and_scan])
        cancelling.add_children([is_cancel_requested, move_home_after_cancel])
        move_out_and_scan.add_children(
            [move_base, scanning, move_home_after_scan])
        scanning.add_children(
            [scan_context_switch, scan_rotate, scan_flash_blue])
        celebrate.add_children([celebrate_flash_green, celebrate_pause])
        return root
 def _takeoff(self):
     self._takeoff_publisher.publish(msg.Empty())
예제 #23
0
    def create_root(idx="1",
                    goal=std_msgs.Empty(),
                    controller_ns="",
                    **kwargs):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        # beahviors
        root = py_trees.composites.Sequence(name="Move")
        blackboard = py_trees.blackboard.Blackboard()
        ## grasp_offset_z = 0.02

        if goal[idx]["primitive_action"] in ['place']:
            if 'object' in goal[idx].keys():
                obj = goal[idx]['object'].encode('ascii', 'ignore')
            elif 'obj' in goal[idx].keys():
                obj = goal[idx]['obj'].encode('ascii', 'ignore')
            else:
                rospy.logerr("MOVE: No place object")
                sys.exit()

            destination = goal[idx]['destination'].encode('ascii', 'ignore')

            if 'destination_offset' in goal[idx].keys():
                destination_offset = goal[idx][
                    'destination_offset']  #.encode('ascii','ignore')
            else:
                destination_offset = [0, 0, 0, 0, 0, 0]
        else:
            return None

        s_init3 = MoveJoint.MOVEJ(name="Init",
                                  controller_ns=controller_ns,
                                  action_goal=blackboard.init_config)

        # ----------------- Place ---------------------
        place = py_trees.composites.Sequence(name="Place")
        pose_est2 = WorldModel.POSE_ESTIMATOR(name="Plan" + idx,
                                              object_dict={
                                                  'target':
                                                  obj,
                                                  'destination':
                                                  destination,
                                                  'destination_offset':
                                                  destination_offset
                                              })
        #from IPython import embed; embed(); sys.exit()
        s_move21 = MovePose.MOVEP(
            name="Top",
            controller_ns=controller_ns,
            action_goal={'pose': "Plan" + idx + "/place_top_pose"})
        s_move22 = MovePose.MOVEP(
            name="Approach",
            controller_ns=controller_ns,
            action_goal={'pose': "Plan" + idx + "/place_pose"})
        s_move23 = Gripper.GOTO(name="Open",
                                controller_ns=controller_ns,
                                action_goal=blackboard.gripper_open_pos,
                                force=blackboard.gripper_open_force)
        s_move24 = MovePose.MOVEP(
            name="Top",
            controller_ns=controller_ns,
            action_goal={'pose': "Plan" + idx + "/place_top_pose"})

        place.add_children(
            [pose_est2, s_move21, s_move22, s_move23, s_move24, s_init3])
        return place
예제 #24
0
 def on_button_start_clicked(self):
     start_message = std_msgs.Empty()
     self._start_pub.publish(start_message)
 def on_button_disable_perception_clicked(self):
     disable_perception_msg = std_msgs.Empty()
     self._disable_perception_pub.publish(disable_perception_msg)
예제 #26
0
 def on_button_vislam_reset_clicked(self):
     vislam_reset_msg = std_msgs.Empty()
     self._vislam_reset_pub.publish(vislam_reset_msg)
예제 #27
0
    def create_root(idx="1",
                    goal=std_msgs.Empty(),
                    controller_ns="",
                    **kwargs):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        # beahviors
        root = py_trees.composites.Sequence(name="Pick")
        blackboard = py_trees.blackboard.Blackboard()

        if goal[idx]["primitive_action"] in ['pick']:
            if 'object' in goal[idx].keys():
                obj = goal[idx]['object'].encode('ascii', 'ignore')
            elif 'obj' in goal[idx].keys():
                obj = goal[idx]['obj'].encode('ascii', 'ignore')
            else:
                rospy.logerr("Pick: No pick object")
                sys.exit()
            ## destination = goal['2']['destination'].encode('ascii','ignore')
        else:
            return None

        # ------------ Compute -------------------------
        s_init1 = MoveJoint.MOVEJ(name="Init",
                                  controller_ns=controller_ns,
                                  action_goal=blackboard.init_config)
        s_init2 = MoveJoint.MOVEJ(name="Init2",
                                  controller_ns=controller_ns,
                                  action_goal=blackboard.init_config)

        # ----------------- Pick ---------------------
        pose_est1 = WorldModel.POSE_ESTIMATOR(name="Plan" + idx,
                                              object_dict={'target': obj})
        s_move11 = MovePose.MOVEP(
            name="Top",
            controller_ns=controller_ns,
            action_goal={'pose': "Plan" + idx + "/grasp_top_pose"})
        s_move12 = Gripper.GOTO(name="Open",
                                controller_ns=controller_ns,
                                action_goal=blackboard.gripper_open_pos,
                                force=blackboard.gripper_open_force)
        s_move13 = MovePose.MOVEP(
            name="Approach",
            controller_ns=controller_ns,
            action_goal={'pose': "Plan" + idx + "/grasp_pose"})
        s_move14 = Gripper.GOTO(name="Close",
                                controller_ns=controller_ns,
                                action_goal=blackboard.gripper_close_pos,
                                force=blackboard.gripper_close_force)
        s_move15 = MovePose.MOVEP(
            name="Top",
            controller_ns=controller_ns,
            action_goal={'pose': "Plan" + idx + "/grasp_top_pose"})

        pick = py_trees.composites.Sequence(name="Pick")
        pick.add_children([
            pose_est1, s_init1, s_move11, s_move12, s_move13, s_move14,
            s_move15, s_init2
        ])

        return pick
예제 #28
0
 def on_button_land_clicked(self):
     land_message = std_msgs.Empty()
     self._land_pub.publish(land_message)
 def publish_button_message(self, publisher):
     publisher.publish(std_msgs.Empty())
예제 #30
0
 def on_button_force_hover_clicked(self):
     force_hover_msg = std_msgs.Empty()
     self._force_hover_pub.publish(force_hover_msg)