def output_decision(self):
	if not self.on:
	    if self.bag_open:
		self.bag.close()
		self.bag_open = False
	    return
	else:
	    if not self.bag_open:
		filename = datetime.datetime.today().strftime("%Y%m%d_%H%M%S") + '.bag'
		rosparam.set_param("/current_bag_file", filename)
		self.bag = rosbag.Bag(filename, 'w')
		self.bag_open = True

	s = self.sensor_values
	a = self.cmd_vel
	e = Event()

	lf = int((-math.pi*30.0/180 - s.angle_min)/s.angle_increment); 
	rf = int((math.pi*30.0/180 - s.angle_min)/s.angle_increment); 
	ls = int((-math.pi*90.0/180 - s.angle_min)/s.angle_increment); 
        rs = int((math.pi*90.0/180 - s.angle_min)/s.angle_increment); 

        e.left_side = 5000.0 if math.isnan(s.ranges[ls]) else s.ranges[ls]*1000;
        e.right_side = 5000.0  if math.isnan(s.ranges[rs]) else s.ranges[rs]*1000;
        e.left_forward = 5000.0 if math.isnan(s.ranges[lf]) else s.ranges[lf]*1000;
        e.right_forward = 5000.0 if math.isnan(s.ranges[rf]) else s.ranges[rf]*1000;
        e.linear_x = a.linear.x
        e.angular_z = a.angular.z

        print(e)

        self._decision.publish(e)
	self.bag.write('/event', e)
Esempio n. 2
0
    def insert_goal(self, pos_x, pos_y, pos_z):
        # 插入一个新点
        # target_points在ORB_SLAM/World坐标系下
        q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz')
        q = Quaternion(*q_angle)
        waypoint = Pose(Point(pos_x, pos_y, pos_z), q)
        waypoint_stamped = PoseStamped()
        waypoint_stamped.header.frame_id = "map"
        waypoint_stamped.header.stamp = rospy.Time(0)
        waypoint_stamped.pose = waypoint
        self.waypoints.append(waypoint_stamped)

        # 转至ORB_SLAM/World坐标系
        rospy.loginfo("获取TF map->ORB_SLAM/World")
        tf_flag = False
        while not tf_flag and not rospy.is_shutdown():
            try:
                t = rospy.Time(0)
                self.listener.waitForTransform("map", "ORB_SLAM/World", t,
                                               rospy.Duration(1.0))
                tf_flag = True
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException, tf.Exception) as e:
                tf_flag = False
                rospy.logwarn("获取TF失败 map->ORB_SLAM/World")
                rospy.logwarn(e)
        rospy.loginfo("获取TF 成功 map->ORB_SLAM/World")
        target_point = self.listener.transformPose(
            "ORB_SLAM/World", waypoint_stamped)
        target_point.header.stamp = rospy.Time.now()
        self.target_points.append(target_point)
        rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
Esempio n. 3
0
 def set_depth(self):
     data = []
     data.append(self.s_d_P.get() * 10**float(self.combo_d_P.get()))
     data.append(self.s_d_I.get() * 10**float(self.combo_d_I.get()))
     data.append(self.s_d_D.get() * 10**float(self.combo_d_D.get()))
     print data
     rosparam.set_param('/PIDpara/depth', str(data))
Esempio n. 4
0
    def output_decision(self):
        if not self.on:
            if self.bag_open:
                self.bag.close()
                self.bag_open = False
            return
        else:
            if not self.bag_open:
                filename = datetime.datetime.today().strftime(
                    "%Y%m%d_%H%M%S") + '.bag'
                rosparam.set_param("/current_bag_file", filename)
                self.bag = rosbag.Bag(filename, 'w')
                self.bag_open = True

        s = self.sensor_values
        a = self.cmd_vel
        e = Event()

        e.left_side = s.left_side
        e.right_side = s.right_side
        e.left_forward = s.left_forward
        e.right_forward = s.right_forward
        e.linear_x = a.linear.x
        e.angular_z = a.angular.z

        self._decision.publish(e)
        self.bag.write('/event', e)
    def add_ok_event(self):
        # Get name for this marker and add to set
        marker_name = self._widget.fiducial_name_edit.text()
        frame = self.current_unknown_marker
        fid = self.id_from_frame(frame)

        self.current_unknown_marker = None
        rospy.loginfo("Adding marker #{} [{}]".format(fid, frame))

        # Add to "seen" list
        self.markers_seen += [fid]
        self.marker_frames[frame] = marker_name

        # Update visualized marker
        self.recolor_marker(fid, [0.0, 1.0, 0.0])

        # Add marker to rosparam
        rosparam_marker_name = "instructor_landmark/{}".format(marker_name)
        rosparam.set_param(rosparam_marker_name, frame)

        # GUI: Append to the list box
        list_txt = "{} ({})".format(marker_name, frame)
        self._widget.fiducial_list.addItem(list_txt)

        # GUI: Reset text field
        self._widget.fiducial_name_edit.setText("")
        self._widget.found_fiducial_id_label_2.setText(
            "Found fiducial with ID: NULL")

        # GUI Remove the "edit name" section of the gui
        self._widget.fiducial_found_widget.hide()
        self._widget.add_fiducial_widget.hide()
Esempio n. 6
0
 def img_callback(self, data):
     img = self.bridge.imgmsg_to_cv2(data)
     cv2.namedWindow("img", cv2.WINDOW_NORMAL)
     self.draw_objects(img)
     cv2.imshow('img', img)
     if cv2.waitKey(1) & 0xFF == 32:
         rosparam.set_param("no_man", True)
 def execute(self, userdata):
     try:
         rospy.loginfo('Executing state NAVIGATION')
         ap_result = userdata.result_message
         coord_list = userdata.coord_in
         m6Control(0.3)
         rosparam.set_param('/move_base/DWAPlannerROS/xy_goal_tolerance', str(0.5))
         print rosparam.get_param('/move_base/DWAPlannerROS/xy_goal_tolerance')
         rospy.sleep(0.1)
         result = navigationAC(coord_list)
         print result
         if self.result == True:
             m6Control(0.4)
             speak('I came close to person')
             #ap_result = result
             #userdata.result_message.data = ap_result
             userdata.result_message.data = self.result
             return 'arrive'
         else:
             speak('I can`t came close to person')
             #ap_result = result
             #userdata.result_message.data = ap_result
             userdata.result_message.data = self.result
             return 'not_arrive'
     except rospy.ROSInterruptException:
         rospy.loginfo('**Interrupted**')
         pass
Esempio n. 8
0
 def set_altitude(self):
     data = []
     data.append(self.s_a_P.get() * 10**float(self.combo_a_P.get()))
     data.append(self.s_a_I.get() * 10**float(self.combo_a_I.get()))
     data.append(self.s_a_D.get() * 10**float(self.combo_a_D.get()))
     print data
     rosparam.set_param('/PIDpara/altitude', str(data))
Esempio n. 9
0
    def insert_goal(self, pos_x, pos_y, pos_z):
        # 插入一个新点
        # target_points在ORB_SLAM/World坐标系下
        q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz')
        q = Quaternion(*q_angle)
        waypoint = Pose(Point(pos_x, pos_y, pos_z), q)
        waypoint_stamped = PoseStamped()
        waypoint_stamped.header.frame_id = "map"
        waypoint_stamped.header.stamp = rospy.Time(0)
        waypoint_stamped.pose = waypoint
        self.waypoints.append(waypoint_stamped)

        # 转至ORB_SLAM/World坐标系
        rospy.loginfo("获取TF map->ORB_SLAM/World")
        tf_flag = False
        while not tf_flag and not rospy.is_shutdown():
            try:
                t = rospy.Time(0)
                self.listener.waitForTransform("map", "ORB_SLAM/World", t,
                                               rospy.Duration(1.0))
                tf_flag = True
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException, tf.Exception) as e:
                tf_flag = False
                rospy.logwarn("获取TF失败 map->ORB_SLAM/World")
                rospy.logwarn(e)
        rospy.loginfo("获取TF 成功 map->ORB_SLAM/World")
        target_point = self.listener.transformPose("ORB_SLAM/World",
                                                   waypoint_stamped)
        target_point.header.stamp = rospy.Time.now()
        self.target_points.append(target_point)
        rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
    def add_ok_event(self):
        # Get name for this marker and add to set
        marker_name = self._widget.fiducial_name_edit.text()
        frame = self.current_unknown_marker
        fid = self.id_from_frame(frame)

        self.current_unknown_marker = None
        rospy.loginfo("Adding marker #{} [{}]".format(fid, frame))

        # Add to "seen" list
        self.markers_seen += [fid]
        self.marker_frames[frame] = marker_name

        # Update visualized marker
        self.recolor_marker(fid, [0.0, 1.0, 0.0])

        # Add marker to rosparam
        rosparam_marker_name = "instructor_landmark/{}".format(marker_name)
        rosparam.set_param(rosparam_marker_name, frame)

        # GUI: Append to the list box
        list_txt = "{} ({})".format(marker_name, frame)
        self._widget.fiducial_list.addItem(list_txt)

        # GUI: Reset text field
        self._widget.fiducial_name_edit.setText("")
        self._widget.found_fiducial_id_label_2.setText("Found fiducial with ID: NULL")

        # GUI Remove the "edit name" section of the gui
        self._widget.fiducial_found_widget.hide()
        self._widget.add_fiducial_widget.hide()
Esempio n. 11
0
 def tidy_up(self):
     """
     Run when this flight state is finished
     Overides pyx4 'tidy_up' method
     """
     rospy.loginfo('tidying up torf_ros.py')
     self.cwssim.tidy_up()
     rospy.logwarn('tidy up torf_ros.py completed')
     rosparam.set_param('cwssim_test_complete', "true")
Esempio n. 12
0
    def set_yaw(self):
        data = []
        data.append(self.s_y.get() * 10**float(self.combo_y.get()))
        data.append(self.s_y_P.get() * 10**float(self.combo_a_P.get()))
        data.append(self.s_y_I.get() * 10**float(self.combo_a_I.get()))
        data.append(self.s_y_D.get() * 10**float(self.combo_a_D.get()))

        print data
        rosparam.set_param('/PIDpara/yaw', str(data))
Esempio n. 13
0
    def callback_param_client(self, command, result):
        """ Callback When a Parameter is Fetched from Client and Needs to Be Set on Server

            Args:
            command: sent command of Type Command -- see Command Class
            result: returned argument from client
        """
        if result is not None:
            result = json.loads(result)
            param_value = result["values"]["value"]
            rosparam.set_param(command.wrapper.name, param_value)
Esempio n. 14
0
    def execute(self,userdata):
        #パラメータ初期化
        self.enemy_pos_from_lider={"enemy_pos":Point(),"is_topic_receive":False}

        #移動時の向きを逃走用(後ろ向き)に設定
        rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "4")

        #逃げる位置計算
        enemy_pos=userdata.enemy_pos_in
        print("enemy_pos",enemy_pos)
        
        #escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v1(enemy_pos.x,enemy_pos.y)#中心挟んで相手の反対地点に逃げるパターン
        #escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v2(enemy_pos.x,enemy_pos.y)#相手の位置によって反対側の決まった地点に逃げるパターン
        #escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v3(enemy_pos.x,enemy_pos.y)#相手の位置によって反対側の決まった地点に逃げるパターン
        escape_pos_x,escape_pos_y,escape_yaw=self.calc_escape_pos_v4(enemy_pos.x,enemy_pos.y,self.my_pose.pos.x,self.my_pose.pos.y)#相手の位置によって反対側の決まった地点に逃げるパターン
       
        #print(escape_pos_x,escape_pos_y,escape_yaw)

        #ゴール設定
        my_move_base.setGoal(self.move_base_client,escape_pos_x,escape_pos_y,escape_yaw)
        
        # rospy終了か、ゴールに着いたらループ抜ける。
        self.is_stop_receive=False
        r = rospy.Rate(5)
        while (not rospy.is_shutdown()) and \
                self.move_base_client.get_state() in [  actionlib_msgs.msg.GoalStatus.ACTIVE,\
                                                        actionlib_msgs.msg.GoalStatus.PENDING]:

            #逃げる途中で,ストップトピックを受け取った場合
            if self.is_stop_receive == True:
                #ループ抜ける
                #目的地設定キャンセル
                self.move_base_client.cancel_goal()
                #移動停止
                for _ in range(5):
                    self.vel_pub.publish(Twist())
                    r.sleep()
                self.is_stop_receive=False
                rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "1")#(None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6)
                return 'is_receiveStopSig'

            #逃げる途中で、敵と遭遇した場合
            if self.enemy_pos_from_lider["is_topic_receive"]==True:
                #前回の敵位置との距離を計算
                dist=self.get_distance(                          enemy_pos.x,                             enemy_pos.y,\
                                    self.enemy_pos_from_lider["enemy_pos"].x,self.enemy_pos_from_lider["enemy_pos"].y)
                # print("dist",dist)
                if dist >= self.CHANGE_ESCAPE_POS_TH: #しきい値を上回っていた場合
                    userdata.enemy_pos_out=self.enemy_pos_from_lider["enemy_pos"]
                    self.move_base_client.cancel_goal()
                    rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "1")#(None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6)
                    return 'is_NearEnemyFound' #抜ける。再計算されて、目的地が変更される。

            
            #TODO:ゴールが壁の中になった時の対応
            r.sleep()
        #目的地についた場合
        rosparam.set_param("/move_base/GlobalPlanner/orientation_mode", "1")#(None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, Backward=4, Leftward=5, Rightward=6)
        return 'is_Gone'
Esempio n. 15
0
def callback(data):
    cv_msgs = msg_list_to_cv(data.camera_msg)
    aruco_detector.set_data(cv_msgs)
    aruco_detector.detect_markers(aruco_dict, aruco_parametrs)
    single_markers, duplicates = aruco_detector.find_duplicates()
    aruco_detector.estimate_pose_single_markers_list(single_markers, dx=dx, dy=dy)
    aruco_detector.estimate_pose_duplicates(duplicates, dx=dx, dy=dy)
    aruco_detector.create_pairs_of_markers()

    distance_between_markers_in_pair = aruco_detector.get_mean_markers_pair_distance()
    if distance_between_markers_in_pair:
        rosparam.set_param("pix_between_markers_pair_centers", str(distance_between_markers_in_pair))
    markers_list = aruco_detector.pairs_of_markers
    msg = prepare_msg(markers_list, duplicates.keys())
    aruco_publisher.publish(msg)
Esempio n. 16
0
    def _launch_node(self):
        package = 'minisim'
        executable = 'minisim_srv' if not self.randomize_maps else 'minisim_srv_standalone'
        node = roslaunch.core.Node(package,
                                   executable,
                                   required=True,
                                   name=self.sim_name,
                                   namespace=self.sim_name,
                                   output='screen')

        if self.randomize_maps:
            rosparam.set_param("/{0}/{0}/map_dir".format(self.sim_name),
                               MinisimEnv.map_dir)

        return MinisimEnv.roslaunch_node_starter.launch(node)
    def __init__(self):
        rospy.loginfo("start init")

        self.loop_rate = rospy.Rate(1)
        self.h_idx=np.zeros(1)
        self.publisher = rospy.Publisher('hypothesis', String, queue_size=1)

        self.TRAIN_DATA_FILES = ['dead_end', 'left', 'right', 'straight', 'threeway_left', 'threeway_center', 'threeway_right']
        NUM_CLASSES = len(self.TRAIN_DATA_FILES)
        self.MAX_LASER_DISTANCE = 30.0

        rosparam.set_param("model_full_path", "/home/docker/catkin_ws/src/intersection_ws/model/NN.h5")
        model_full_path = rosparam.get_param("tensorflow/model_full_path")
        self.model = keras.models.load_model(model_full_path)

        rospy.loginfo("finish init")
Esempio n. 18
0
def start_ros():  # pragma: no cover
    """
    Starts ROS utilities and cleanup thread in the app process.
    """
    root_logger.info("Starting ROS node")
    # Block until connection to ROS master is established, and initialize a backend node
    rospy.init_node("nrp_backend")
    rosparam.set_param("/use_sim_time", "true")

    rospy_thread = Thread(target=rospy.spin)
    rospy_thread.setDaemon(True)
    rospy_thread.start()

    root_logger.info("Starting cleanup thread")
    cleanup_thread = Thread(target=clean_simulations)
    cleanup_thread.setDaemon(True)
    cleanup_thread.start()
Esempio n. 19
0
def table_coeffs_cb(data):
    openni_frame = data.header.frame_id
    ps = PointStamped()
    ps.header.frame_id = openni_frame
    #print data.values
    h_openni = -data.values[3]/data.values[2]
    ps.point.z = h_openni
    try:
        listener.waitForTransform('base_footprint', openni_frame, rospy.Time.now(), rospy.Duration(4.0))
        #(trans, rot) = listener.lookupTransform('/base_link', openni_frame, rospy.Time())
        ps = listener.transformPoint('base_footprint', ps)
    except:
        return
    if math.isnan(ps.point.z) or np.isnan(ps.point.z):
        return
    h_floor = ps.point.z + OFFSET
    print 'table height: ', h_floor
    rosparam.set_param('/table_height', str(h_floor))
    call(['rosnode', 'kill', 'table_detect_node'])
    rospy.signal_shutdown('table height set')
Esempio n. 20
0
def table_coeffs_cb(data):
    openni_frame = data.header.frame_id
    ps = PointStamped()
    ps.header.frame_id = openni_frame
    #print data.values
    h_openni = -data.values[3] / data.values[2]
    ps.point.z = h_openni
    try:
        listener.waitForTransform('base_footprint', openni_frame,
                                  rospy.Time.now(), rospy.Duration(4.0))
        #(trans, rot) = listener.lookupTransform('/base_link', openni_frame, rospy.Time())
        ps = listener.transformPoint('base_footprint', ps)
    except:
        return
    if math.isnan(ps.point.z) or np.isnan(ps.point.z):
        return
    h_floor = ps.point.z + OFFSET
    print 'table height: ', h_floor
    rosparam.set_param('/table_height', str(h_floor))
    call(['rosnode', 'kill', 'table_detect_node'])
    rospy.signal_shutdown('table height set')
Esempio n. 21
0
    def __init__(self, name, filters=OrderedDict()):
        self.name = name
        self.node = Node("ros_vision", "filter_chain_node.py", name)
        self.reset_params()
        self.node.run()
        create_filter_srv_name = '/%s/create_filter' % name
        rospy.wait_for_service(create_filter_srv_name)
        self.create_filter = rospy.ServiceProxy(create_filter_srv_name, CreateFilter)

        i = 0
        for filter_name in filters.keys():
            i += 1

            if 'type' in filters[filter_name].keys():
                filter_type = filters[filter_name]['type']
                del filters[filter_name]['type']

                for parameter_name in filters[filter_name].keys():
                    rosparam.set_param('/%s/%s/%s' % (name, filter_name, parameter_name), str(filters[filter_name][parameter_name]))

                self.create_filter(filter_name, filter_type, i)
Esempio n. 22
0
    def logger(self):
        if not self.mode:
            if self.bagfile_open:
                self.bag.close()
                self.bagfile_open = False
            return
        else:
            print("教示中・・・")
            if not self.bagfile_open:
                name = "PFoE_" + datetime.datetime.today().strftime(
                    "%Y_%m_%d_%H_%M_%S") + ".bag"
                rosparam.set_param("/bagfile", name)
                self.bag = rosbag.Bag(name, 'w')
                self.bagfile_open = True

        self.e.left_forward = self.sensor_values.left_forward
        self.e.left_side = self.sensor_values.left_side
        self.e.right_side = self.sensor_values.right_side
        self.e.right_forward = self.sensor_values.right_forward
        self.e.linear_x = self.cmd_vel.linear.x
        self.e.angular_z = self.cmd_vel.angular.z

        self.event_pub.publish(self.e)
        self.bag.write('/event', self.e)
def main():
    global unload_controller_service,load_controller_service,switch_controller_service,shutdown_timeout

    args = parse_args(rospy.myargv()[1:])

    wait_for_topic = args.wait_for
    autostart = 1 if not args.stopped else 0
    robot_namespace = args.namespace or ""
    timeout = args.timeout
    shutdown_timeout = args.shutdown_timeout

    rospy.init_node('spawner', anonymous=True)

    # add a '/' to the namespace if needed
    if robot_namespace and robot_namespace[-1] != '/':
        robot_namespace = robot_namespace+'/'

    # set service names based on namespace
    load_controller_service = robot_namespace+"controller_manager/load_controller"
    unload_controller_service = robot_namespace+"controller_manager/unload_controller"
    switch_controller_service = robot_namespace+"controller_manager/switch_controller"

    try:
        # loader
        rospy.loginfo("Controller Spawner: Waiting for service "+load_controller_service)
        rospy.wait_for_service(load_controller_service, timeout=timeout)
        load_controller = rospy.ServiceProxy(load_controller_service, LoadController)

        # switcher
        rospy.loginfo("Controller Spawner: Waiting for service "+switch_controller_service)
        rospy.wait_for_service(switch_controller_service, timeout=timeout)
        switch_controller = rospy.ServiceProxy(switch_controller_service, SwitchController)

        # unloader
        # NOTE: We check for the unloader's existence here, although its used on shutdown because shutdown
        # should be fast. Further, we're interested in knowing if we have a compliant controller manager from
        # early on
        rospy.loginfo("Controller Spawner: Waiting for service "+unload_controller_service)
        rospy.wait_for_service(unload_controller_service, timeout=timeout)

    except rospy.exceptions.ROSException:
        rospy.logwarn("Controller Spawner couldn't find the expected controller_manager ROS interface.")
        return

    if wait_for_topic:
        # This has to be a list since Python has a peculiar mechanism to determine
        # whether a variable is local to a function or not:
        # if the variable is assigned in the body of the function, then it is
        # assumed to be local. Modifying a mutable object (like a list)
        # works around this debatable "design choice".
        wait_for_topic_result = [None]

        def wait_for_topic_cb(msg):
            wait_for_topic_result[0] = msg
            rospy.logdebug("Heard from wait-for topic: %s" % str(msg.data))
        rospy.Subscriber(wait_for_topic, Bool, wait_for_topic_cb)
        started_waiting = time.time()

        # We might not have received any time messages yet
        warned_about_not_hearing_anything = False
        while not wait_for_topic_result[0]:
            time.sleep(0.01)
            if rospy.is_shutdown():
                return
            if not warned_about_not_hearing_anything:
                if time.time() - started_waiting > timeout:
                    warned_about_not_hearing_anything = True
                    rospy.logwarn("Controller Spawner hasn't heard anything from its \"wait for\" topic (%s)" % \
                                  wait_for_topic)
        while not wait_for_topic_result[0].data:
            time.sleep(0.01)
            if rospy.is_shutdown():
                return

    # find yaml files to load
    controllers = []
    for name in args.controllers:
        if os.path.exists(name):
            # load yaml file onto the parameter server, using the namespace specified in the yaml file
            rosparam.set_param("",open(name))
            # list the controllers to be loaded
            name_yaml = yaml.load(open(name))
            for controller in name_yaml:
                controllers.append(controller)
        else:
            controllers.append(name)

    # load controllers
    for name in controllers:
        rospy.loginfo("Loading controller: "+name)
        resp = load_controller(name)
        if resp.ok != 0:
            loaded.append(name)
        else:
            time.sleep(1) # give error message a chance to get out
            rospy.logerr("Failed to load %s" % name)

    rospy.loginfo("Controller Spawner: Loaded controllers: %s" % ', '.join(loaded))

    if rospy.is_shutdown():
        return

    # start controllers is requested
    if autostart:
        resp = switch_controller(loaded, [], 2)
        if resp.ok != 0:
            rospy.loginfo("Started controllers: %s" % ', '.join(loaded))
        else:
            rospy.logerr("Failed to start controllers: %s" % ', '.join(loaded))

    rospy.spin()
Esempio n. 24
0
def main():
    global unload_controller_service, load_controller_service, switch_controller_service

    args = parse_args(rospy.myargv()[1:])

    wait_for_topic = args.wait_for
    robot_namespace = args.namespace or ""
    timeout = args.timeout

    rospy.init_node('spawner', anonymous=True)

    start_controllers = rospy.get_param("~start_controllers", [])
    stop_controllers = rospy.get_param("~stop_controllers", [])

    if start_controllers:
        rospy.loginfo("Controller spawner: Starting controllers -- " +
                      str(start_controllers))
    if stop_controllers:
        rospy.loginfo("Controller spawner: Stopping controllers -- " +
                      str(stop_controllers))

    if args.shutdown_timeout is not None:
        rospy.logwarn("DEPRECATION warning: --shutdown-timeout has no effect.")

    # add a '/' to the namespace if needed
    if robot_namespace and robot_namespace[-1] != '/':
        robot_namespace = robot_namespace + '/'

    # set service names based on namespace
    load_controller_service = robot_namespace + "controller_manager/load_controller"
    unload_controller_service = robot_namespace + "controller_manager/unload_controller"
    switch_controller_service = robot_namespace + "controller_manager/switch_controller"

    try:
        # loader
        rospy.loginfo("Controller Spawner: Waiting for service " +
                      load_controller_service)
        rospy.wait_for_service(load_controller_service, timeout=timeout)
        load_controller = rospy.ServiceProxy(load_controller_service,
                                             LoadController)

        # switcher
        rospy.loginfo("Controller Spawner: Waiting for service " +
                      switch_controller_service)
        rospy.wait_for_service(switch_controller_service, timeout=timeout)
        switch_controller = rospy.ServiceProxy(switch_controller_service,
                                               SwitchController)

        # unloader
        # NOTE: We check for the unloader's existence here, although its used on shutdown because shutdown
        # should be fast. Further, we're interested in knowing if we have a compliant controller manager from
        # early on
        rospy.loginfo("Controller Spawner: Waiting for service " +
                      unload_controller_service)
        rospy.wait_for_service(unload_controller_service, timeout=timeout)

    except rospy.exceptions.ROSException:
        rospy.logwarn(
            "Controller Spawner couldn't find the expected controller_manager ROS interface."
        )
        return

    if wait_for_topic:
        # This has to be a list since Python has a peculiar mechanism to determine
        # whether a variable is local to a function or not:
        # if the variable is assigned in the body of the function, then it is
        # assumed to be local. Modifying a mutable object (like a list)
        # works around this debatable "design choice".
        wait_for_topic_result = [None]

        def wait_for_topic_cb(msg):
            wait_for_topic_result[0] = msg
            rospy.logdebug("Heard from wait-for topic: %s" % str(msg.data))

        rospy.Subscriber(wait_for_topic, Bool, wait_for_topic_cb)
        started_waiting = time.time()

        # We might not have received any time messages yet
        warned_about_not_hearing_anything = False
        while not wait_for_topic_result[0]:
            time.sleep(0.01)
            if rospy.is_shutdown():
                return
            if not warned_about_not_hearing_anything:
                if time.time() - started_waiting > timeout:
                    warned_about_not_hearing_anything = True
                    rospy.logwarn("Controller Spawner hasn't heard anything from its \"wait for\" topic (%s)" % \
                                      wait_for_topic)
        while not wait_for_topic_result[0].data:
            time.sleep(0.01)
            if rospy.is_shutdown():
                return

    # hook for unloading controllers on shutdown
    rospy.on_shutdown(shutdown)

    # find yaml files to load
    controllers = []
    for name in start_controllers + stop_controllers:
        if os.path.exists(name):
            # load yaml file onto the parameter server, using the namespace specified in the yaml file
            rosparam.set_param("", open(name))
            # list the controllers to be loaded
            name_yaml = yaml.load(open(name))
            for controller in name_yaml:
                controllers.append(controller)
        else:
            controllers.append(name)

    # load controllers
    for name in controllers:
        rospy.loginfo("Loading controller: " + name)
        resp = load_controller(name)
        if resp.ok != 0:
            loaded.append(name)
        else:
            time.sleep(1)  # give error message a chance to get out
            rospy.logerr("Failed to load %s" % name)

    rospy.loginfo("Controller Spawner: Loaded controllers: %s" %
                  ', '.join(loaded))

    if rospy.is_shutdown():
        return

    # start controllers is requested
    start_controllers = [l for l in loaded if l in start_controllers]
    resp = switch_controller(start_controllers, [], 2)
    if resp.ok != 0:
        rospy.loginfo("Started controllers: %s" % ', '.join(start_controllers))
    else:
        rospy.logerr("Failed to start controllers: %s" %
                     ', '.join(start_controllers))

    rospy.spin()
Esempio n. 25
0
"""
				ros_param.py

python script that will load ROS Parameters 
from a YAML file, parse them and will upload
them to ROS 'Parametr Server'. 
"""

import rospy
import rosparam
import yaml

with open("validation_params.yaml", 'r') as stream:
    data = yaml.load(stream)

while len(data) > 0:
    parameter = data.popitem()
    rosparam.set_param(parameter[0], str(parameter[1]))
Esempio n. 26
0
 def set_yaw(self):
     data = self.s_y.get() * 10**float(self.combo_y.get())
     print data
     rosparam.set_param('/tune/yaw', str(data))
Esempio n. 27
0
    def torf_logic(self):
        """
        Main logic for torf model used in conjunction with the CWSSIM view matching pipeline.

        Handles CWSSIM image processing depending on the flight state

        """
        while not rospy.is_shutdown() and self.node_alive:

            # Get flight state to see what phase of the mission we are in
            this_state_label = self.commander._flight_instruction.state_label

            # if we are in the outbound/learning flight phase then add new images to our memory database
            if this_state_label == 'Head_out':
                self.cwssim_state = CWSSIM_STATE.OUTBOUND
                if not self.image_q.empty():
                    rospy.logdebug('adding image')
                    self.cwssim.add_image(self.image_q.get())
                    self.publish_cwssim_status()
                # check that we are not lagging behind the image processing task
                if self.image_q.qsize() > 5:
                    rospy.logwarn(
                        'getting behind on image processing - queue size is: {}'.format(self.image_q.qsize()))

            # if we are in the inbound/homing flight phase then interrogate new images for familiarity against the
            # images in our memory database
            elif this_state_label == 'Sweep_state':
                self.cwssim_state = CWSSIM_STATE.INBOUND
                if self.new_image_evt:
                    self.new_image_evt = False
                    if self.use_multiprocessing:
                        if self.multiprocessing_status == MULTIPROCESSING_STATUS.INITIALISED:
                            self.cwssim_score, self.cwssim_score_idx = \
                                self.cwssim.max_query_image_with_index_mp(self.this_image)
                        else:
                            rospy.logwarn_throttle(1, "memory bank not initialised yet - can't do multiprocessing")
                            if self.multiprocessing_status == MULTIPROCESSING_STATUS.NOT_INIT:
                                rospy.logwarn_throttle(1, "trying to initialised memory bank but this should have "
                                                          "already been done - there is an error in the code")
                                self.initialise_memory_bank_for_multiprocessing()
                    else:
                        rospy.logwarn_throttle(5, 'not using multiprocessing, fewer images possible')
                        self.cwssim_score, self.cwssim_score_idx = \
                            self.cwssim.max_query_image_with_index_mp(self.this_image)
                    self.publish_cwssim_status()

            elif (this_state_label == 'Turn around time') or (this_state_label == 'Return to start'):
                self.cwssim_state = CWSSIM_STATE.TURNING_AROUND
                self.new_image_evt = False                         # clear the last image
                if self.multiprocessing_status == MULTIPROCESSING_STATUS.NOT_INIT:
                    if self.need2reverse_image_idxs:
                        rospy.logwarn("flip images before preparing memory bank for multiprocessing")
                    else:
                        rospy.loginfo("preparing memory bank for multiprocessing")
                        self.initialise_memory_bank_for_multiprocessing()
            else:
                self.cwssim_state = CWSSIM_STATE.INTERUPTION

            # if we n
            if self.need2reverse_image_idxs and \
                (this_state_label=='Turn around time' or this_state_label=='Return to start'):
                rospy.logwarn('started reversing CWSSIM image ids')
                self.cwssim.reverse_image_ids()
                self.need2reverse_image_idxs = False
                rospy.logwarn('Completed reversing CWSSIM image ids')

            try:
                self.ros_rate.sleep()
            except:
                # prevent gabble being printed to the terminal
                break

        # do not do tidy up - if pyx4 also calls this it seems to cause problems
        # self.tidy_up()
        rospy.logwarn('setting mission complete parameter to true')
        rosparam.set_param('cwssim_test_complete', "true")
Esempio n. 28
0
 def __set_param(self, name, value):
     rosparam.set_param(name, str(value))
Esempio n. 29
0
 def register_dnsmasq(self, IP):
     self.dnsmasqIP = IP
     rosparam.set_param("{}/dnsmasqIP".format(self.master_handle), IP)
     rospy.loginfo("Registered dnsmasq server at {}".format(IP))
Esempio n. 30
0
def map_mux():

    rospy.init_node("map_mux", anonymous=True)
    # Topic Subscriber
    rospy.Subscriber("map1", OccupancyGrid, addMap1)
    rospy.Subscriber("map2", OccupancyGrid, addMap2)
    rospy.Subscriber("map3", OccupancyGrid, addMap3)
    # Topic Publisher
    topic = rospy.resolve_name("map")
    pub = rospy.Publisher("map", OccupancyGrid)
    topic = rospy.resolve_name("map_metadata")
    pub_metadata = rospy.Publisher("map_metadata", MapMetaData)
    initialPosePub = rospy.Publisher("initialpose",  PoseWithCovarianceStamped)
    global change_map
    global flag
    # Service Servers
    s = rospy.Service('change_map', ChangeMap, changeMapfunc)
    s = rospy.Service('static_map', GetMap, staticMapfunc)
    # Service Clients
    service_floor_switch = rospy.ServiceProxy("floor_switch", ChangeMap)
    rp = rospkg.RosPack()
    try:
        path = rp.get_path("map_mux") + "/src"
    except rospkg.ResourceNotFound:
        print "package not found"
    print path
    print "waiting for floor_switch to come up"
    #rospy.wait_for_service("floor_switch")


    r = rospy.Rate(.1)
    old_change_map = 0

    while not rospy.is_shutdown():
        if( map1 == None  and map2 == None and map3 == None):
            print "you need to provide 3 maps on topics map1 map2 map3 from a launch file."
        #if (change_map != old_change_map):
        if (1):
            if (change_map == 1 and map1 != None):
                rospy.loginfo("changing to 1")
                pub.publish(map1)
                pub_metadata.publish(map1.info)
            if (change_map == 2 and map2 != None  and flag ==1 ):
                rospy.loginfo("changing to 2")
                pub.publish(map2)
                pub_metadata.publish(map2.info)
                #print str(type(initalPositionFloor2))
                try:
                    initialPosePub.publish(initalPositionFloor2)
                    rosparam.set_param("/segbot_logical_navigator/door_file",
                            rp.get_path("bwi_kr") + "/config/multi_map/atrium_doors.yaml")
                    rosparam.set_param("/segbot_logical_navigator/location_file",
                            rp.get_path("bwi_kr") + "/config/multi_map/atrium_locations.yaml")
                    rosparam.set_param("/segbot_logical_navigator/object_file",
                            rp.get_path("bwi_kr") + "/config/multi_map/atrium_objects.yaml")
                    rosparam.set_param("/segbot_logical_navigator/map_file",
                            rp.get_path("map_mux") + "/maps/atrium_with_elevators.yaml")

                    resp_floor_switch = service_floor_switch(change_map)
                except rospy.ServiceException:
                    print "floor switch service call failed"
                print "done with floor 2"
                flag = 0
            if (change_map == 3 and map3 != None and flag ==1 ):
                rospy.loginfo("changing to 3")
                pub.publish(map3)
                pub_metadata.publish(map3.info)
                try:
                    initialPosePub.publish(initalPositionFloor3)
                    rosparam.set_param("/segbot_logical_navigator/door_file",
                            rp.get_path("bwi_kr") + "/config/multi_map/3ne_doors.yaml")
                    rosparam.set_param("/segbot_logical_navigator/location_file",
                            rp.get_path("bwi_kr") + "/config/multi_map/3ne_locations.yaml")
                    rosparam.set_param("/segbot_logical_navigator/object_file",
                            rp.get_path("bwi_kr") + "/config/multi_map/3ne_objects.yaml")
                    rosparam.set_param("/segbot_logical_navigator/map_file",
                            rp.get_path("map_mux") + "/maps/map_whole2_with_elevators.yaml")

                    resp_floor_switch = service_floor_switch(change_map)
                except rospy.ServiceException:
                    print "floor switch service call failed"
                print "done with floor 3"
                flag = 0
        old_change_map = change_map
Esempio n. 31
0
    def load_targets_task(self):
        if not os.path.exists(self.nav_points_file):
            self.target_points = []
            self.waypoints = list()

        with open(self.nav_points_file, "r") as nav_data_file:
            nav_data_str = nav_data_file.readline()
            self.target_points = []
            while len(nav_data_str) != 0:
                pos_x = float(nav_data_str.split(" ")[0])
                pos_y = float(nav_data_str.split(" ")[1])
                pos_z = float(nav_data_str.split(" ")[2])
                self.target_points.append([pos_x, pos_y, pos_z])
                nav_data_str = nav_data_file.readline()

        self.waypoints = list()
        # nav_path_points = []
        # with open("/home/xiaoqiang/slamdb/path.csv", "r") as nav_data_file:
        #     nav_data_str = nav_data_file.readline()
        #     while len(nav_data_str) != 0:
        #         pos_x = float(nav_data_str.split(" ")[0])
        #         pos_y = float(nav_data_str.split(" ")[1])
        #         pos_z = float(nav_data_str.split(" ")[2])
        #         nav_path_points.append([pos_x, pos_y, pos_z])
        #         nav_data_str = nav_data_file.readline()
        # nav_path_points_2d = [[point[0], point[2]]
        #                       for point in nav_path_points]
        for point in self.target_points:
            pose_in_world = PoseStamped()
            pose_in_world.header.frame_id = "ORB_SLAM/World"
            pose_in_world.header.stamp = rospy.Time(0)
            pose_in_world.pose.position = Point(point[0], point[1], point[2])
            # axis = self.get_target_direction(
            #     [point[0], point[2]], nav_path_points_2d)
            # q_angle = quaternion_from_euler(
            #     math.pi / 2, -math.atan2(axis[1], axis[0]), 0, axes='sxyz')
            q_angle = quaternion_from_euler(0, 0, 0, axes='sxyz')
            pose_in_world.pose.orientation = Quaternion(*q_angle)

            # 转至map坐标系
            rospy.loginfo("获取ORB_SLAM/World->TF map")
            tf_flag = False
            while not tf_flag and not rospy.is_shutdown(
            ) and self.running_flag:
                try:
                    t = rospy.Time(0)
                    self.listener.waitForTransform("ORB_SLAM/World", "map", t,
                                                   rospy.Duration(1.0))
                    tf_flag = True
                except (tf.LookupException, tf.ConnectivityException,
                        tf.ExtrapolationException, tf.Exception) as e:
                    tf_flag = False
                    rospy.logwarn("获取TF失败 ORB_SLAM/World->map")
                    rospy.logwarn(e)
            if not self.running_flag:
                self.load_targets_exited_flag = True
                return
            rospy.loginfo("获取TF 成功 ORB_SLAM/World->map")
            waypoint = self.listener.transformPose("map", pose_in_world)
            self.waypoints.append(waypoint)
        self.load_targets_exited_flag = True
        # 通过全局规划器,计算目标点的朝向
        rospy.loginfo("waiting for move_base/make_plan service")
        rospy.wait_for_service("/move_base/make_plan")
        rospy.loginfo("waiting for move_base/make_plan service succeed")
        make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
        for waypoint in self.waypoints:
            req = GetPlanRequest()
            req.start = self.waypoints[0]
            if waypoint == self.waypoints[0]:
                req.start = self.waypoints[1]
            req.goal = waypoint
            req.tolerance = 0.1
            res = make_plan(req)
            if len(res.plan.poses) > 10:
                res.plan.poses = res.plan.poses[:-4]
            plan_path_2d = [[point.pose.position.x, point.pose.position.y]
                            for point in res.plan.poses]
            angle = self.get_target_direction(
                [waypoint.pose.position.x, waypoint.pose.position.y],
                plan_path_2d)
            q_angle = quaternion_from_euler(0,
                                            0,
                                            math.atan2(angle[1], angle[0]) +
                                            math.pi,
                                            axes='sxyz')
            waypoint.pose.orientation = Quaternion(*q_angle)
        rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
Esempio n. 32
0
 def reset_goals(self):
     self.current_goal_id = -1
     self.goal_status = "FREE"
     self.load_targets_task()
     rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
    def start(self):
        self.delay = rospy.get_param('~delay', 10)
        self.prefilter_size = rospy.get_param('~prefilter_size', '23')
        self.prefilter_cap = rospy.get_param('~prefilter_cap', '33')
        self.correlation_window_size = rospy.get_param(
            '~correlation_window_size', '41')
        self.min_disparity = rospy.get_param('~min_disparity', '44')
        self.disparity_range = rospy.get_param('~disparity_range', '64')
        self.uniqueness_ratio = rospy.get_param('~uniqueness_ratio', '15.0')
        self.texture_threshold = rospy.get_param('~texture_threshold', '10')
        self.speckle_size = rospy.get_param('~speckle_size', '356')
        self.speckle_range = rospy.get_param('~speckle_range', '7')
        self.restore_time = rospy.get_param('~restore_time', 5)
        self.heartbeat_period = rospy.get_param('~heartbeat_period', 5.0)
        self.heartbeat_timeout = rospy.get_param('~heartbeat_timeout', 10.0)
        rospy.loginfo(
            "\n"
            "=================================================================\n"
            "Started 3DPC Extractor with the following paramterers:\n"
            "=================================================================\n"
            "ID                      : " + self.id + "\n"
            "Delay (s)               : " + str(self.delay) + "\n"
            "Prefilter size          : " + self.prefilter_size + "\n"
            "Prefilter cap           : " + self.prefilter_cap + "\n"
            "Correlation Window Size : " + self.correlation_window_size + "\n"
            "Min. Disparity          : " + self.min_disparity + "\n"
            "Disparity Range         : " + self.disparity_range + "\n"
            "Uniqueness Ratio        : " + self.uniqueness_ratio + "\n"
            "Texture Threshold       : " + self.texture_threshold + "\n"
            "Speckle Size            : " + self.speckle_size + "\n"
            "Speckle Range           : " + self.speckle_range + "\n"
            "Bond Restore Time (s)   : " + str(self.restore_time) + "\n"
            "Heartbeat Period (s)    : " + str(self.heartbeat_period) + "\n"
            "Heartbeat Timeout (s)   : " + str(self.heartbeat_timeout) + "\n"
            "New 3DPC Msg Queue size : " + str(self.new_3dpc_msg_queue_size) +
            "\n"
            "=================================================================\n"
        )

        time.sleep(self.delay)

        self.node_publisher = rospy.Publisher(
            "/new_3dpc_extractor",
            New3DPCExtractor.New3DPCExtractor,
            queue_size=self.new_3dpc_msg_queue_size)

        node = roslaunch.core.Node(
            package="stereo_image_proc",
            node_type="stereo_image_proc",
            name=self.id,
            machine_name="localhost",
            args="",
            respawn=True,
            remap_args=[("/left/image_color", self.id + "/left/image_color"),
                        ("/left/image_mono", self.id + "/left/image_mono"),
                        ("/left/image_rect", self.id + "/left/image_rect"),
                        ("/left/image_rect_color",
                         self.id + "/left/image_rect_color"),
                        ("/right/image_color", self.id + "/right/image_color"),
                        ("/right/image_mono", self.id + "/right/image_mono"),
                        ("/right/image_rect", self.id + "/right/image_rect"),
                        ("/right/image_rect_color",
                         self.id + "/right/image_rect_color")],
            env_args=None,
            output=None,
            cwd=None,
            launch_prefix=None,
            required=False)

        self.roslaunch_instance = roslaunch.scriptapi.ROSLaunch()
        self.roslaunch_instance.start()

        param_path = "/" + self.id + "/"

        rosparam.set_param(param_path + "prefilter_size", self.prefilter_size)
        rosparam.set_param(param_path + "prefilter_cap", self.prefilter_cap)
        rosparam.set_param(param_path + "correlation_window_size",
                           self.correlation_window_size)
        rosparam.set_param(param_path + "min_disparity", self.min_disparity)
        rosparam.set_param(param_path + "disparity_range",
                           self.disparity_range)
        rosparam.set_param(param_path + "uniqueness_ratio",
                           self.uniqueness_ratio)
        rosparam.set_param(param_path + "texture_threshold",
                           self.texture_threshold)
        rosparam.set_param(param_path + "speckle_size", self.speckle_size)
        rosparam.set_param(param_path + "speckle_range", self.speckle_range)

        self.roslaunch_instance.launch(node)

        new_extractor_msg = New3DPCExtractor.New3DPCExtractor()
        new_extractor_msg.id = self.id
        new_extractor_msg.topic = self.id + "/bond"

        self.node_publisher.publish(new_extractor_msg)

        bond = bondpy.Bond(self.id + "/bond", self.id)

        bond.set_heartbeat_period(self.heartbeat_period)
        bond.set_heartbeat_timeout(self.heartbeat_timeout)

        rospy.Timer(rospy.Duration(self.restore_time), self.restore_bond)

        bond.start()
 def test_rosparam(self,s):
     rosparam.set_param("/dadayeh_param",s)
Esempio n. 35
0
        package_name = sys.argv[1] + "_support"
        command = sys.argv[2]
        if command == "-l":
            list_work_objects(package_name)
            exit()
        else:
            work_name = command

    rospy.init_node("publish_work")

    scene = moveit_commander.PlanningSceneInterface()
    rospy.sleep(1.0)  # wait for the above things to setup

    remove_all_objects(scene)

    work = parse_urdf_file(package_name, work_name)
    publish_parsed_urdf(work, scene)

    # parse and load task to parameter server
    rospack = rospkg.RosPack()
    filename = work_name + ".irl"
    task_path = rospack.get_path(package_name) + REL_TASK_PATH + filename
    rosparam.set_param("/planning_task_path", task_path)

    task = parse_file(task_path)
    plotter = Plotter(ref_frame="/world")
    # plotter = Plotter(ref_frame="/work") # TODO fix setup 3
    show_task(plotter, task)

    print("Done!")
Esempio n. 36
0
    def load_targets_task(self):
        if not os.path.exists(self.nav_points_file):
            self.target_points = []
            self.waypoints = list()

        with open(self.nav_points_file, "r") as nav_data_file:
            nav_data_str = nav_data_file.readline()
            self.target_points = []
            while len(nav_data_str) != 0:
                pos_x = float(nav_data_str.split(" ")[0])
                pos_y = float(nav_data_str.split(" ")[1])
                pos_z = float(nav_data_str.split(" ")[2])
                self.target_points.append([pos_x, pos_y, pos_z])
                nav_data_str = nav_data_file.readline()

        self.waypoints = list()
        # nav_path_points = []
        # with open("/home/xiaoqiang/slamdb/path.csv", "r") as nav_data_file:
        #     nav_data_str = nav_data_file.readline()
        #     while len(nav_data_str) != 0:
        #         pos_x = float(nav_data_str.split(" ")[0])
        #         pos_y = float(nav_data_str.split(" ")[1])
        #         pos_z = float(nav_data_str.split(" ")[2])
        #         nav_path_points.append([pos_x, pos_y, pos_z])
        #         nav_data_str = nav_data_file.readline()
        # nav_path_points_2d = [[point[0], point[2]]
        #                       for point in nav_path_points]
        for point in self.target_points:
            pose_in_world = PoseStamped()
            pose_in_world.header.frame_id = "ORB_SLAM/World"
            pose_in_world.header.stamp = rospy.Time(0)
            pose_in_world.pose.position = Point(point[0], point[1], point[2])
            # axis = self.get_target_direction(
            #     [point[0], point[2]], nav_path_points_2d)
            # q_angle = quaternion_from_euler(
            #     math.pi / 2, -math.atan2(axis[1], axis[0]), 0, axes='sxyz')
            q_angle = quaternion_from_euler(
                0, 0, 0, axes='sxyz')
            pose_in_world.pose.orientation = Quaternion(*q_angle)

            # 转至map坐标系
            rospy.loginfo("获取ORB_SLAM/World->TF map")
            tf_flag = False
            while not tf_flag and not rospy.is_shutdown() and self.running_flag:
                try:
                    t = rospy.Time(0)
                    self.listener.waitForTransform("ORB_SLAM/World", "map", t,
                                                   rospy.Duration(1.0))
                    tf_flag = True
                except (tf.LookupException, tf.ConnectivityException,
                        tf.ExtrapolationException, tf.Exception) as e:
                    tf_flag = False
                    rospy.logwarn("获取TF失败 ORB_SLAM/World->map")
                    rospy.logwarn(e)
            if not self.running_flag:
                self.load_targets_exited_flag = True
                return
            rospy.loginfo("获取TF 成功 ORB_SLAM/World->map")
            waypoint = self.listener.transformPose(
                "map", pose_in_world)
            self.waypoints.append(waypoint)
        self.load_targets_exited_flag = True
        # 通过全局规划器,计算目标点的朝向
        rospy.loginfo("waiting for move_base/make_plan service")
        rospy.wait_for_service("/move_base/make_plan")
        rospy.loginfo("waiting for move_base/make_plan service succeed")
        make_plan = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
        for waypoint in self.waypoints:
            req = GetPlanRequest()
            req.start = self.waypoints[0]
            if waypoint == self.waypoints[0]:
                req.start = self.waypoints[1]
            req.goal = waypoint
            req.tolerance = 0.1
            res = make_plan(req)
            if len(res.plan.poses) > 10:
                res.plan.poses = res.plan.poses[:-4]
            plan_path_2d = [[point.pose.position.x, point.pose.position.y]
                            for point in res.plan.poses]
            angle = self.get_target_direction(
                [waypoint.pose.position.x, waypoint.pose.position.y], plan_path_2d)
            q_angle = quaternion_from_euler(0, 0, math.atan2(
                angle[1], angle[0]) + math.pi, axes='sxyz')
            waypoint.pose.orientation = Quaternion(*q_angle)
        rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))
Esempio n. 37
0
 def reset_goals(self):
     self.current_goal_id = -1
     self.goal_status = "FREE"
     self.load_targets_task()
     rosparam.set_param("/galileo/goal_num", str(len(self.target_points)))