Пример #1
0
def handle_auction_server_callback(auction_req):

    global role_assigned, actual_role
    global master_server, buyer_server


    # update number of messages in parameter server
    if rospy.has_param('/num_messages'):
        num_messages = rospy.get_param('/num_messages')
        num_messages += 2
        rospy.set_param('/num_messages', num_messages)

   # Clean up
    if rospy.has_param('/auction_closed'):
        if rospy.get_param('/auction_closed') == True:
            role_assigned = False

    rospy.loginfo(rospy.get_name()+' '+str(role_assigned))

    # avoid node to take another role
    if not role_assigned:
        role_assigned = True

        if auction_req.role == 'be_auctioneer':
            return auctioneer.handle_auction_server_callback(auction_req)

        elif auction_req.role == 'be_buyer':
            return buyer_k_saap.handle_auction_server_callback(auction_req)

        else:
            return {'response_info':'invalid role requested'}

    else:
        return {'response_info':'node already have a role'}
 def __init__(self):
     rospy.set_param('/nav_experiments/topics', ['/base_controller/command'])
     self.tf = tf.TransformListener()
     self.target_frame = '/base_footprint'
     self.pub = rospy.Publisher('/goal_state', PolygonStamped, latch=True)
     self._as = actionlib.SimpleActionServer('/move_base', MoveBaseAction, execute_cb=self.execute_cb, auto_start=False)
     self._as.start()
Пример #3
0
 def handler(self, respawn_request):
     node_requesting = respawn_request.node_name
     faulty_port = respawn_request.port_name
     #Get both assigned port numbers:
     control_port = rospy.get_param('AER_Driver/control_port')
     sensor_port = rospy.get_param('Sensor_Monitor/sensor_port')
     #Get a list of actually available ports
     actual_ports = glob.glob('/dev/ttyACM*')
     if (len(actual_ports) > 1):
         #Handle the respawn request
         available_ports = []
         for port_number in actual_ports:
             if port_number != sensor_port:
                 available_ports.append(port_number)
         #Make sure we have a port available
         if (len(available_ports) == 0):
             #If not, send an error code
             return RespawnResponse(2)
         #If we have available ports
         kill_cmd = "rosnode kill /" + node_requesting
         spawn_cmd = "rosrun AER " + node_requesting + ".py&"
         #Kill requesting node
         subprocess.call(kill_cmd, shell=True)
         #Set new parameter
         param_name = 'AER_Driver/control_port'
         rospy.set_param(param_name, available_ports[0])
         #Rerun the node
         subprocess.call(spawn_cmd, shell=True)
     else:
         #Send an error code
         return RespawnResponse(1)
Пример #4
0
def init():
    global scaleStatusPub, camOdomPub,lisener,scaleOrbFlag,scale,odom_combined_tf
    global velPub
    rospy.init_node("orb_scale", anonymous=True)
    scaleOrbFlag=rospy.get_param('/orb2base_scaleFlag',False)
    if scaleOrbFlag:
        #load scale value from scale.txt
        file_path=os.path.split(os.path.realpath(__file__))[0]
        fp3=open("%s/scale.txt"%(file_path),'r+')
        for line in fp3:
            value_list=line.split(" ")
        scale=float(value_list[0])
        if scale<=0.000001:
            scale=0.
            scaleOrbFlag=False
        else:
            rospy.set_param('/orb2base_scale',scale)
            fp3.close
        print "scale: "+str(scale)
    lisener=tf.TransformListener()
    odom_combined_tf=tf.TransformBroadcaster()
    rospy.Subscriber("/system_monitor/report", Status, systemStatusHandler)
    scaleStatusPub = rospy.Publisher('/orb_scale/scaleStatus', Bool , queue_size=5)
    camOdomPub = rospy.Publisher("/ORB_SLAM/Odom", Odometry, queue_size=5)
    rospy.Subscriber("/ORB_SLAM/Camera", Pose, cameraOdom)
    velPub = rospy.Publisher('/cmd_vel', Twist , queue_size=5)
    t=threading.Thread(target=publish_Tfs)
    t.start()
 def execute(self, userdata):
     aux=userdata.current_robot_pose
     aux.pose.position.x
     aux.pose.position.y
     yaw=userdata.current_robot_yaw
     rospy.loginfo(OKGREEN+"I Have a new point"+ENDC)
     if (userdata.objectOrientation== 'right') :
         yaw=yaw+(math.pi/2)
     if (userdata.objectOrientation== 'left'):
         yaw=yaw-(math.pi/2)
     if (userdata.objectOrientation== 'back'):
         yaw=yaw-math.pi
     if (userdata.objectOrientation=='front'):
         yaw=yaw
     
     
     
     
     value=["submap_0",userdata.objectName,aux.pose.position.x,
            aux.pose.position.y,yaw]
     
     rospy.loginfo(OKGREEN+str(value)+ENDC)
     rospy.set_param("/mmap/poi/submap_0/"+str(userdata.objectName),value)
     #"/restaurant/submap_0/"+str(userdata.objectName)
     return 'succeeded'
    def pause_bt_button_clicked(self):
        '''

        :param self:
        :return:
        '''
        rospy.set_param('/apc/task_manager/running', False)
Пример #7
0
	def Run(self):
		if not self.sss.parse:
			rospy.loginfo("Testing Sound modes...")
			rospy.loginfo("If you can't hear something, check soundcard number (#card) with:")
			rospy.loginfo("    cat /proc/asound/cards")
			rospy.loginfo("Initialize card with:")
			rospy.loginfo("    alsactrl init #card")
		for i in range(1):
			self.sss.say(["Hello, my name is Care-O-bot."])
			
			self.sss.say(["Hello, my name is Care-O-bot.","How are you?"])
			
			self.sss.say("sent00")
			
			self.sss.say("sent00","de")

			self.sss.say(123)
			self.sss.say([123])

			self.sss.say(["Hello, my name is Care-O-bot.","How are you?"],False)
			self.sss.sleep(1)
			self.sss.say(["This is a non blocking voice."])

			rospy.set_param("script_server/sound/language","de")
			self.sss.play("grasp_tutorial_01")
		
			rospy.set_param("script_server/sound/language","en")
			self.sss.play("grasp_tutorial_01")
Пример #8
0
    def __init__(self):
        self.json_file = rospy.get_param('~json', None)
        self.is_apc2016 = rospy.get_param('~is_apc2016', True)
        self.gripper = rospy.get_param('~gripper', 'gripper2016')
        self.max_weight = rospy.get_param('~max_weight', -1)
        if self.json_file is None:
            rospy.logerr('must set json file path to ~json')
            return
        data = json.load(open(self.json_file))
        self.work_order = {}
        work_order_list = []
        for order in data['work_order']:
            bin_ = order['bin'].split('_')[1].lower()
            self.work_order[bin_] = order['item']
            work_order_list.append({'bin': bin_, 'item': order['item']})
        rospy.set_param('~work_order', work_order_list)

        self.object_data = None
        if self.is_apc2016:
            self.object_data = jsk_apc2016_common.get_object_data()

        self.bin_contents = jsk_apc2016_common.get_bin_contents(param='~bin_contents')

        self.msg = self.get_work_order_msg()

        self.pub_left = rospy.Publisher(
            '~left_hand', WorkOrderArray, queue_size=1)
        self.pub_right = rospy.Publisher(
            '~right_hand', WorkOrderArray, queue_size=1)
        rospy.Service('~update_target', UpdateTarget, self._update_target_cb)
        self.updated = False
        rospy.Timer(rospy.Duration(rospy.get_param('~duration', 1)), self._timer_cb)
Пример #9
0
    def execute(self, userdata=None):

        # Store current base position
        base_loc = self._robot.base.get_location()
        base_pose = base_loc.frame

        # Create automatic side detection state and execute
        self._robot.speech.speak("I am now going to look for the table", block=False)
        automatic_side_detection = AutomaticSideDetection2(self._robot)
        side = automatic_side_detection.execute({})
        self._robot.head.look_at_standing_person()

        self._robot.speech.speak("The {} is to my {}".format(self._location_id, side))

        self._robot.head.cancel_goal()

        if side == "left":
            base_pose.M.DoRotZ(math.pi / 2)
        elif side == "right":
            base_pose.M.DoRotZ(-math.pi / 2)

        # Add to param server
        loc_dict = {'x': base_pose.p.x(), 'y': base_pose.p.y(), 'phi': base_pose.M.GetRPY()[2]}
        rospy.set_param("/restaurant_locations/{name}".format(name=self._location_id), loc_dict)

        self._visualize_location(base_pose, self._location_id)
        self._robot.ed.update_entity(id=self._location_id, frame_stamped=FrameStamped(base_pose, "/map"),
                                     type="waypoint")

        return "done"
Пример #10
0
    def __init__(self, object_list, is_debug=False):
        self._is_reload = rospy.get_param('/pr2_pbd_interaction/isReload')

        self._exp_number = None
        self._selected_step = 0
        self._object_list = object_list

        if (is_debug):
            self._exp_number = rospy.get_param(
                                '/pr2_pbd_interaction/experimentNumber')
            self._data_dir = self._get_data_dir(self._exp_number)
            if (not os.path.exists(self._data_dir)):
                os.mkdir(self._data_dir)
        else:
            self._get_participant_id()
        rospy.set_param('data_directory', self._data_dir)

        self.actions = dict()
        self.current_action_index = 0

        if (self._is_reload):
            self._load_session_state(object_list)
            rospy.loginfo("Session state loaded.")

        n_actions = dict()
        for k in self.actions.keys():
            n_actions[str(k)] = self.actions[k].n_frames()

        self._state_publisher = rospy.Publisher('experiment_state',
                                                ExperimentState)
        rospy.Service('get_experiment_state', GetExperimentState,
                      self.get_experiment_state_cb)

        self._update_experiment_state()
Пример #11
0
def callback_stop(dat):
    global listen_active
    listen_active= not dat.data
    rospy.set_param('/sense/stt/get_listen_active',listen_active)
    pubActive = rospy.Publisher('/act/robot/set_listen_led', Bool)
    pubActive.publish(listen_active)
    rospy.loginfo("listen active:"+str(listen_active))
def main():
    """
    Run the main loop, by instatiating a PendulumSimulator, and then
    calling ros.spin
    """
    rospy.init_node('pendulum_simulator')  # , log_level=rospy.INFO)

    # check what the value of the links param is
    links_bool = rospy.get_param('links', False)
    if not rospy.has_param('links'):
        rospy.set_param('links', links_bool)

    count = 1

    try:
        system = create_systems(2, link_length='1.0', link_mass='2.0', frequency='0.5', amplitude='0.5', damping='0.1')
        user = User(USER_NUMBER, MAX_TRIALS, STARTING_TRUST, ALPHA, INPUT_DEVICE)
        r = rospy.Rate(100)
        while user.current_trial_num < user.max_num_trials:
            rospy.loginfo("Trial number: %d", count)
            sim = PendulumSimulator(system, user.current_trust, user.current_trial_num)
            while not sim.finished:
                r.sleep()
            rospy.loginfo("current trust: %f", user.current_trust)
            user.update_trust(sim.task_trust, sim.tcost)
            rospy.loginfo("new current trust: %f, task trust: %f", user.current_trust, sim.task_trust)
            del sim
            count = count + 1
            time.sleep(2)

    except rospy.ROSInterruptException:
        pass

    rospy.loginfo('Session Complete')
Пример #13
0
    def __init__(self):
        self.indexHtmlTemplate = Template(filename="templates/indexTemplate.html")

        self.lang = rospy.get_param("/system_lang", "en")

        # self.lang='en'
        # Load default dict
        fp = open("lang/" + self.lang + ".txt", "r")
        self.language = json.load(fp, "utf-8")
        fp.close()
        # Load specific dict if different to english
        if self.lang != "en":
            try:
                fp = open("lang/" + self.lang + ".txt", "r")
                #            self.language.update(json.load(fp))

                self.language.update(json.load(fp))
                fp.close()
            except IOError:
                print "Language error"
                self.lang = "en"

        # Post the system lang to ROS and set the Param
        if self.lang == "en" or self.lang == "es" or self.lang == "it":
            rospy.set_param("/system_lang", self.lang)
            lang_pub = rospy.Publisher("/system_lang", String)
            lang_pub.publish(String(self.lang))
            print "PUBLISHING NEW LANG: " + self.lang

        """
Пример #14
0
def privateParam(param_name, default_value=None):
    response = rospy.get_param("~" + param_name, default_value)
    if response == None:
        raise Exception("ERROR: Mandatory '%s' parameter not found."%(param_name))
    else:
        rospy.set_param("~" + param_name, response)
    return response
Пример #15
0
	def Run(self):
		if not self.sss.parse:
			rospy.loginfo("Testing Sound modes...")
		for i in range(1):
			self.sss.say(["Hello, my name is Care-O-bot."])
			
			self.sss.say(["Hello, my name is Care-O-bot.","How are you?"])
			
			self.sss.say("sent00")
			
			self.sss.say("sent00","de")

			self.sss.say(123)
			self.sss.say([123])

			self.sss.say(['Once Upon A Time, there was a Shoemaker named Zerbo. He very carefully handcrafted every pair of shoes. Each pair of shoes was made especially for each person, and they were made to fit perfectly.'],False)

			self.sss.sleep(1)
			self.sss.say(["This is a non blocking voice which should be played in parallel to the previous text."])

			rospy.set_param("script_server/sound/language","de")
			self.sss.play("grasp_tutorial_01")
		
			rospy.set_param("script_server/sound/language","en")
			self.sss.play("grasp_tutorial_01")
Пример #16
0
def listener():
   rospy.init_node('vp_ardrone2_joy_node') 
   rospy.loginfo("vp_ardrone2_joy - OK!")   
   if not rospy.has_param("/vp_ardrone2/flag2"):
     rospy.set_param("/vp_ardrone2/flag2",0) 
   sub = rospy.Subscriber("joy",Joy,controller)
   rospy.spin()
Пример #17
0
    def __init__(self, hostname, username, password, interface):
        self.hostname = hostname
        self.username = username
        self.password = password
        self.interface = interface

        self.passmgr = urllib2.HTTPPasswordMgrWithDefaultRealm()
        self.passmgr.add_password(realm=None,
                             uri="http://%s/"%(hostname),
                             user=username,
                             passwd=password)
        self.auth_handler = urllib2.HTTPBasicAuthHandler(self.passmgr)
        self.opener = urllib2.build_opener(self.auth_handler)
        urllib2.install_opener(self.opener)

        self.current_config = {}
        self.get_current_config()

        self.set_req_args = {}
        self.set_req_args["action"] = "Apply"
        self.set_req_args["submit_type"] = ""
        self.set_req_args["change_action"] = ""
        self.set_req_args["commit"] = "1"
        self.set_req_args["next_page"] = ""

        self.avail_txpower_list = self.get_avail_txpower_list()

        node_name = rospy.get_name()
        for param_name in self.current_config:
            param_full_name = node_name + "/" + param_name
            if not rospy.has_param(param_full_name):
                rospy.set_param(param_full_name, self.current_config[param_name])
Пример #18
0
        def kinect_cb(self,rawDepthImage):

		# If data is requested for localization
		if(rospy.get_param('visual_tracker')==rospy.get_name()):
		
			self.cloud= read_points(rawDepthImage, field_names=None, skip_nans=False, uvs=[])

			# Convert a cloud generator to a list
			self.points = []
			for pt in self.cloud:
				pt = list(pt)
				pt.append(1)
				self.points.append(pt)

			nearestPoint=self.getNearestPoint()
			print nearestPoint[2]


			# Create a new message to be sent to ROSToArduino
			messageToROS=fromObserver_msg()
			messageToROS.observerID=rospy.get_name()
			messageToROS.coordinates=(nearestPoint)
			self.KinectToROS_pub.publish(messageToROS)
			
			# Reset the visual_tracker parameter
			rospy.set_param('visual_tracker','')
Пример #19
0
def controller(data):
    # проверка takeoff - кнопка start/10
    if(data.buttons[9]==1):  # взлет
        rospy.loginfo("взлет")
        pub1=rospy.Publisher('ardrone/takeoff', Empty)
        pub1.publish()
        time.sleep(1)
    elif(data.buttons[8]==1):  # посадка- кнопка back/9
        rospy.loginfo("посадка")
        pub1=rospy.Publisher('ardrone/land', Empty)
        pub1.publish()
        time.sleep(1)
    elif(data.buttons[5]==1):  # посадка- кнопка RB/6
        rospy.loginfo("посадка")
        pub1=rospy.Publisher('ardrone/land', Empty)
        pub1.publish()
        rospy.set_param("/vp_ardrone2/flag2",1)
        time.sleep(1)
    elif(data.buttons[4]==1):  # flattrim- кнопка RB/5
        rospy.loginfo("flattrim")
        rospy.set_param("/vp_ardrone2/flag2",1)
        serv1=rospy.ServiceProxy('ardrone/flattrim',std_srvs.srv.Empty)
        res1=serv1();
    else:             # другие клавиши
        pass
Пример #20
0
    def __init__(self):
        # create all required variables:
        self.running_flag = False
        self.ref_time = None
        self.ref_state = None
        # self.ref_func = None
        self.ref_arr = None
        self.tbase = rospy.Time.now()
        self.X = None
        
        self.first_time = True  
        self.get_intergral_time  = True
        
        self.time_gap = 0
        
        self.Einx = 0
        self.Einy = 0

        # create all publishers and subscribers:
        self.cmd_pub = rospy.Publisher("cmd_vel_raw", Twist, queue_size=1)
        
        #set the transfrom broadcaster to publish the velocity transfrom from spatial frame to body frame
        self.br = tf.TransformBroadcaster()
        self.timer = rospy.Timer(rospy.Duration(0.005), self.timercb)
        #intialize the subscriber to subscribe the data from odom/, this odom/ is used as feedback for driving the base 
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.odomcb, queue_size=10)
        self.traj_sub = rospy.Subscriber("base_controller/follow_joint_trajectory/goal", FollowJointTrajectoryActionGoal,
                         self.trajcb, queue_size=1) 
                         
        
        rospy.set_param('return_to_base', False)
        
                                                   
        return
def launch_heartbeat_operator_node():
    rospy.init_node('dronemap_heartbeat_operator', anonymous=True)
    
    # get parameters from the ros parameters server
    platform_ip_address = rospy.get_param('/cloud_platform_ip_address')
    platform_port_number = rospy.get_param('/cloud_platform_port_number')
    buffer_size = rospy.get_param('/buffer_size')

    rate = rospy.Rate(50)
    if not rospy.has_param("/drone_registered"):
        rospy.set_param("/drone_registered", False)

    while not rospy.get_param("/drone_registered"):
        rospy.loginfo(rospy.get_caller_id() + ": Still not registered yet.")
        rate.sleep()

    print "yes, registered"
    
    # get the formatter
    formatter = create_message_formatter()

    # start the operator
    client = TCPClient("dronemap_heartbeat_operator", platform_ip_address, platform_port_number, formatter, buffer_size)
    HeartbeatControlOperator(client)
    
    # Keep python from exiting until this node is stopped
    rospy.spin()
Пример #22
0
 def odom_res_changed(self, value):
     self.settings.odom_res_slider.blockSignals(True)
     self.settings.odom_res_slider.setValue(int(100*value))
     self.settings.odom_res_slider.blockSignals(False)
     self.settings.odom_res_box.setValue(value)
     self.robot.odometer.res = value
     rospy.set_param('~encoder_resolution', value)
Пример #23
0
 def gps_noise_changed(self, value):
     self.settings.gps_noise_slider.blockSignals(True)
     self.settings.gps_noise_slider.setValue(int(100*value))
     self.settings.gps_noise_slider.blockSignals(False)
     self.settings.gps_noise_box.setValue(value)
     self.robot.gps.noise = value
     rospy.set_param('~gps_noise', value)
Пример #24
0
    def __init__(self):
        rospy.init_node("enemy_manager")
        self.ns = rospy.get_namespace()

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

        names = ['t1', 't2', 't3']
        self.nodes = {}
        self.processes = {}
        y = 0
        for name in names:
            y += 1
            param_name = "/" + name + "/y"
            if self.ns != "/":
                param_name = self.ns + param_name
            rospy.set_param(param_name, y)
            self.nodes[name] = roslaunch.core.Node("grid_defense", "enemy.py",
                                                   name=name, namespace=self.ns,
                                                   args="")
            self.processes[name] = self.launch.launch(self.nodes[name])

        # while not rospy.is_shutdown():
        #    rospy.sleep(1.0)
        rospy.spin()

        for name in names:
            rospy.loginfo("stopping " + name)
            self.processes[name].stop()
Пример #25
0
   def __init__(self):
      # find USB rockets and set them up
      manager = RocketManager()
      manager.acquire_devices()
      if len(manager.launchers) == 0:
         rospy.logerr("Failed to find rocket launcher")
         exit(-1)
      # TODO: handle more than one launcher
      self.launcher = manager.launchers[0]
      # launcher.usb_debug = True
   
      # Calibration
      if rospy.has_param('avg'):
         print 'Calibration found on prameter server'
         self.avg = rospy.get_param('avg')
         self.var = rospy.get_param('var')
      else:
         print 'No calibration found. calibrating'
         (self.avg, self.var) = self.calibrate()
         rospy.set_param('avg', self.avg)
         rospy.set_param('var', self.var)

      self.pos_home()
      self.alpha_target = ALPHA_HOME
      self.beta_target = BETA_HOME
      self.firing = False
      self.moving = False
   
      rospy.Service('rocket_fire', Empty, self.fire)
      rospy.Subscriber('rocket_command', RocketCommand, self.position)
      self.pub = rospy.Publisher('rocket_position', RocketPosition)
      rospy.logdebug('rocket_driver ready')
Пример #26
0
    def __init__(self):
        self.bInitialized = False
        
        # initialize
        self.name = 'acquirevoltages2msg'
        rospy.init_node(self.name, anonymous=False)
        self.nodename = rospy.get_name()
        self.namespace = rospy.get_namespace()
        
        # Load the parameters.
        self.params = rospy.get_param('%s' % self.nodename.rstrip('/'), {})
        self.defaults = {'channels_ai':[0,1,2,3,4,5,6,7],
                         'channels_di':[0,1,2,3,4,5,6,7]}
        SetDict().set_dict_with_preserve(self.params, self.defaults)
        rospy.set_param('%s' % self.nodename.rstrip('/'), self.params)
        
        self.command = None

        # Messages & Services.
        self.topicAI = '%s/ai' % self.namespace.rstrip('/')
        self.pubAI       = rospy.Publisher(self.topicAI, MsgAnalogIn)
        self.topicDI = '%s/di' % self.namespace.rstrip('/')
        self.pubDI       = rospy.Publisher(self.topicDI, MsgDigitalIn)
        self.subCommand  = rospy.Subscriber('%s/command' % self.nodename.rstrip('/'), String, self.command_callback, queue_size=1000)
        
        self.get_ai = rospy.ServiceProxy('get_ai', SrvPhidgetsInterfaceKitGetAI)
        self.get_di = rospy.ServiceProxy('get_di', SrvPhidgetsInterfaceKitGetDI)
        
        rospy.sleep(1) # Allow time to connect.
        
        self.bInitialized = True
Пример #27
0
    def run(self):
        while 1:
            try:
                path,val = self.queue.get(block=True, timeout=1.0/self._freq)
                with rosgobject.get_parameter_lock():
                    rospy.set_param(path,val)
            except Queue.Empty:
                pass

            with rosgobject.get_parameter_lock():
                try:
                    val = rospy.get_param(self._path)
                    has_param = True
                except KeyError:
                    val = None
                    has_param = False

            create_param = False
            if has_param:
                if self._val is None:
                    self._existscallback(self._path)
                if val != self._val:
                    self._changecallback(self._path, val)
                    self._val = val
            elif self._create is not None:
                with rosgobject.get_parameter_lock():
                    rospy.set_param(self._path, self._create)
                    create_param = True

            if create_param:
                self._log.info("set parameter %s to %r" % (self._path, self._create))
                self._existscallback(self._path)
                self._changecallback(self._path, self._create)
Пример #28
0
 def gyro_noise_changed(self, value):
     self.settings.gyro_noise_slider.blockSignals(True)
     self.settings.gyro_noise_slider.setValue(int(100*value))
     self.settings.gyro_noise_slider.blockSignals(False)
     self.settings.gyro_noise_box.setValue(value)
     self.robot.gyroscope.noise = math.radians(value)
     rospy.set_param('~gyro_noise', math.radians(value))
Пример #29
0
 def go_to_start(self, midpoint, pub):
     r = 10.0
     rate = rospy.Rate(r)
     for i in range(0, 15 * int(r)):
         out_msg = QuadPositionDerived()
         out_msg.x = 0.0
         out_msg.y = 0.0
         out_msg.z = 0.6
         out_msg.yaw = 0
         out_msg.x_vel = 0.0
         out_msg.y_vel = 0.0
         out_msg.z_vel = 0.0
         out_msg.yaw_vel = 0
         out_msg.x_acc = 0.0
         out_msg.y_acc = 0.0
         out_msg.z_acc = 0.0
         out_msg.yaw_acc = 0
         pub.publish(out_msg)
         rate.sleep()
     target_point = [0.0, 0.0, 0.0]
     target_point[0] = midpoint[0] + self.radius
     target_point[1] = midpoint[1]
     target_point[2] = midpoint[2]
     outpos = self.transform_coordinates(target_point, self.theta)
     rospy.set_param("trajectory_generator/start_point", [0.0, 0.0, 0.6])
     rospy.set_param("trajectory_generator/end_point", outpos)
     sl_gen = StraightLineGen()
     sl_gen.generate()
     return outpos
Пример #30
0
def set_windowsize(n=10):
    """
    Set the size of the history to maintain for each sensor
    :param n: The number of values to keep
    :return: None
    """
    rospy.set_param('aggregation_windowsize', n)
Пример #31
0
    def launchWrapper(self, virtual_robot, ros_namespace, frequency=200):
        """
        Launches the ROS wrapper

        Parameters:
            virtual_robot - The instance of the simulated model
            ros_namespace - The ROS namespace to be added before the ROS topics
            advertized and subscribed
            frequency - The frequency of the ROS rate that will be used to pace
            the wrapper's main loop
        """
        if MISSING_IMPORT is not None:
            raise pybullet.error(MISSING_IMPORT)

        self.virtual_robot = virtual_robot
        self.ros_namespace = ros_namespace
        self.frequency = frequency

        rospy.init_node("qibullet_wrapper",
                        anonymous=True,
                        disable_signals=False)

        # Upload the robot description to the ros parameter server
        try:
            if isinstance(self.virtual_robot, PepperVirtual):
                robot_name = "pepper"
            elif isinstance(self.virtual_robot, NaoVirtual):
                robot_name = "nao"
            elif isinstance(self.virtual_robot, RomeoVirtual):
                robot_name = "romeo"
            else:
                raise pybullet.error(
                    "Unknown robot type, wont set robot description")

            package_path = roslib.packages.get_pkg_dir("naoqi_driver")
            urdf_path = package_path + "/share/urdf/" + robot_name + ".urdf"

            with open(urdf_path, 'r') as file:
                robot_description = file.read()

            rospy.set_param("/robot_description", robot_description)

        except IOError as e:
            raise pybullet.error("Could not retrieve robot descrition: " +
                                 str(e))

        # Launch the robot state publisher
        robot_state_publisher = roslaunch.core.Node("robot_state_publisher",
                                                    "robot_state_publisher")

        self.roslauncher = roslaunch.scriptapi.ROSLaunch()
        self.roslauncher.start()
        self.roslauncher.launch(robot_state_publisher)

        # Initialize the ROS publisher and subscribers
        self._initPublishers()
        self._initSubscribers()

        # Launch the wrapper's main loop
        self._wrapper_termination = False
        self.spin_thread = Thread(target=self._spin)
        self.spin_thread.start()
Пример #32
0
    def __init__(self):
        self.node_name = 'cont_anti_instagram_node'
        robot_name = rospy.get_param("~veh",
                                     "")  #to read the name always reliably

        self.active = True
        self.locked = False

        # Initialize publishers and subsImportError: cannot import namecribers
        self.pub_trafo = rospy.Publisher("~transform",
                                         AntiInstagramTransform,
                                         queue_size=1)
        self.pub_trafo_CB = rospy.Publisher("~colorBalanceTrafo",
                                            AntiInstagramTransform_CB,
                                            queue_size=1)
        self.pub_health = rospy.Publisher("~health",
                                          AntiInstagramHealth,
                                          queue_size=1,
                                          latch=True)
        self.pub_mask = rospy.Publisher("~mask", Image, queue_size=1)
        self.pub_geomImage = rospy.Publisher("~geomImage", Image, queue_size=1)
        self.sub_image = rospy.Subscriber(
            '/ubiquityrobot/camera_node/image_raw/compressed',
            CompressedImage,
            self.cbNewImage,
            queue_size=1)

        # Verbose option
        self.verbose = rospy.get_param('line_detector_node/verbose', False)

        # Read parameters
        self.interval = self.setupParameter("~ai_interval", 10)
        self.fancyGeom = self.setupParameter("~fancyGeom", False)
        self.n_centers = self.setupParameter("~n_centers", 6)
        self.blur = self.setupParameter("~blur", 'median')
        self.resize = self.setupParameter("~resize", 0.2)
        self.blur_kernel = self.setupParameter("~blur_kernel", 5)
        self.cb_percentage = self.setupParameter("~cb_percentage", 50)
        self.trafo_mode = self.setupParameter("~trafo_mode", 'cb')
        if not (self.trafo_mode == "cb" or self.trafo_mode == "lin"
                or self.trafo_mode == "both"):
            rospy.loginfo(
                "cannot understand argument 'trafo_mode'. set to 'both' ")
            self.trafo_mode == "both"
            rospy.set_param(
                "~trafo_mode",
                "both")  # Write to parameter server for transparancy
        #    rospy.loginfo("[%s] %s = %s " % (self.node_name, "~trafo_mode", "both"))

        # Initialize health message
        self.health = AntiInstagramHealth()

        # Initialize transform message
        self.transform = AntiInstagramTransform()

        # initialize color balance transform message
        self.transform_CB = AntiInstagramTransform_CB()

        # initialize AI class
        self.ai = AntiInstagram()
        self.ai.setupKM(self.n_centers, self.blur, 1, self.blur_kernel)

        # initialize msg bridge
        self.bridge = CvBridge()

        self.image_msg = None

        # timer for continuous image process
        self.timer_init = rospy.Timer(rospy.Duration(self.interval),
                                      self.processImage)
        rospy.loginfo('ai: Looking for initial trafo.')

        # bool to switch from initialisation to continuous mode
        self.initialized = False

        self.max_it_1 = 10
        self.max_it_2 = 2

        # container for mask and maskedImage
        self.mask255 = []
        self.geomImage = []
        self.cb_percentage = rospy.Timer(rospy.Duration.from_sec(1.0),
                                         self.updateParams)
Пример #33
0
 def setupParameter(self, param_name, default_value):
     value = rospy.get_param(param_name, default_value)
     rospy.set_param(param_name,
                     value)  #Write to parameter server for transparancy
     rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
     return value
def main():
    urdfname = "/data/ros/yue_ws/src/tcst_pkg/urdf/ur5.urdf"
    filename = "/data/ros/yue_ws/src/tcst_pkg/yaml/cam_300_industry_20200518.yaml"
    # urdfname="/data/ros/ur_ws/src/universal_robot/ur_description/urdf/ur5.urdf"
    desiruv = []
    lambda1 = -1.166666
    detat = 0.05
    z = 0.92
    ace = 50
    vel = 0.1
    urt = 0
    ratet = 10
    p0 = IBVSControl(filename, lambda1, urdfname)

    ur_reader = Urposition()
    ur_sub = rospy.Subscriber("/joint_states", JointState, ur_reader.callback)

    u_error_pub = rospy.Publisher("/feature_u_error", Float64, queue_size=10)
    v_error_pub = rospy.Publisher("/feature_v_error", Float64, queue_size=10)
    z_depth_pub = rospy.Publisher("/camera_depth", Float64, queue_size=10)

    ur_pub = rospy.Publisher("/ur_driver/URScript", String, queue_size=10)

    down_to_q = []
    desire_joint_angular = [124.08, -86.65, 60.29, 295.00, -89.38, 166.73]
    start_angular = [44.87, -70.67, 40.06, 299.46, -89.69, 182.71]
    cartisian_feedback = p0.getpi(desire_joint_angular)
    start_angular_back = p0.getpi(start_angular)

    rate = rospy.Rate(ratet)

    count_for_desire = 0
    place_count = 2
    while not rospy.is_shutdown():

        desiruv = []
        uvlist = []
        open_ibvs_flag = rospy.get_param("open_ibvs_flag")
        open_go_to_desire = rospy.get_param("open_go_to_desire")
        print("open_ibvs_flag,open_go_to_desire", open_ibvs_flag,
              open_go_to_desire)
        # print(p0.uv_list_buffer)
        if open_ibvs_flag == 1:
            print("Object picking")
            if len(p0.uv_list_buffer) != 0:

                uvlist.append(p0.uv_list_buffer[-1])

                desiruv.append([333, 241])

                feature_error = p0.get_feature_error(desiruv, uvlist[0])
                print "Ibvs is ok?---", p0.return_error_ok(
                    feature_error.tolist()[0][0],
                    feature_error.tolist()[0][1])
                if p0.return_error_ok(feature_error.tolist()[0][0],
                                      feature_error.tolist()[0][1]) == False:
                    print "feature error\n", feature_error
                    u_error_pub.publish(feature_error.tolist()[0][0])
                    v_error_pub.publish(feature_error.tolist()[0][1])

                    q_now = ur_reader.ave_ur_pose
                    detaangular = p0.get_deta_joint_angular(
                        detat, uvlist, z, desiruv, uvlist[0], q_now)

                    q_pub_now = p0.get_joint_angular(q_now, detaangular)

                    print "q_now\n", q_now
                    print "q_pub_now\n", q_pub_now
                    down_to_q = q_pub_now
                    ss = "movej([" + str(q_pub_now[0]) + "," + str(
                        q_pub_now[1]) + "," + str(q_pub_now[2]) + "," + str(
                            q_pub_now[3]) + "," + str(
                                q_pub_now[4]) + "," + str(
                                    q_pub_now[5]) + "]," + "a=" + str(
                                        ace) + "," + "v=" + str(
                                            vel) + "," + "t=" + str(urt) + ")"
                    ur_pub.publish(ss)
                if p0.return_error_ok(feature_error.tolist()[0][0],
                                      feature_error.tolist()[0][1]) == True:
                    rospy.set_param("/open_go_to_object", 0)
                    # down_to_q=[0.6840241781464097, -1.7849443418136726, -1.2719627004551626, -1.671977857427553, 1.6020880964542237, 2.9842358676782488]
                    if len(down_to_q) != 0:
                        temp_q0 = p0.get_inverse_to_box(down_to_q, 0, 0, -0.15)
                        p0.move_ur_l(ur_pub, temp_q0, 0.2, ace, urt)
                        time.sleep(1)
                        x_length = +0.112
                        y_length = +0.04  #-0.005
                        z_depth = 0
                        temp_q1 = p0.get_inverse_to_box(
                            temp_q0, x_length, y_length, z_depth)
                        p0.move_ur_l(ur_pub, temp_q1, 0.2, ace, urt)
                        time.sleep(1)
                        x_length = +0.002
                        y_length = +0.002  #-0.005
                        depth = 0
                        temp_q2 = p0.get_inverse_to_box(
                            temp_q1, x_length, y_length, depth)
                        p0.move_ur_l(ur_pub, temp_q2, 0.2, ace, urt)
                        time.sleep(1)
                        temp_q3 = p0.get_inverse_to_box(temp_q2, 0, 0, -0.19)
                        p0.move_ur_l(ur_pub, temp_q3, 0.2, ace, urt)
                        time.sleep(1)
                        os.system(
                            "rostopic pub /io_state std_msgs/String '55C8010155' -1"
                        )
                        p0.move_ur_l(ur_pub, temp_q2, 0.2, ace, urt)
                        time.sleep(1)
                        p0.move_ur_l(ur_pub, cartisian_feedback, 0.2, ace, urt)
                        time.sleep(5)
                        rospy.set_param("open_ibvs_flag", 0)

                        rospy.set_param("/open_go_to_desire", 1)
                        uvlist = []
                        p0.uv_list_buffer = []

        # if open_go_to_desire==1 and open_ibvs_flag==0:
        #     print("Desire pick",len(p0.uv_desire_list_buffer))
        #     if len(p0.uv_desire_list_buffer)!=0:
        #         # desire_object=p0.image_space_planning([569,474],[44,65],3,3)
        #         uvlist.append([p0.uv_desire_list_buffer[-1][0],p0.uv_desire_list_buffer[-1][1]])
        #         desiruv.append([333,241])
        #         # desiruv.append([550,339])

        #         feature_error=p0.get_feature_error(desiruv,uvlist[0])
        #         print "Ibvs is ok?---",p0.return_error_ok_desire(feature_error.tolist()[0][0],feature_error.tolist()[0][1])
        #         if p0.return_error_ok_desire(feature_error.tolist()[0][0],feature_error.tolist()[0][1])==False:
        #             print "feature error\n",feature_error
        #             u_error_pub.publish(feature_error.tolist()[0][0])
        #             v_error_pub.publish(feature_error.tolist()[0][1])

        #             q_now=ur_reader.ave_ur_pose

        #             detaangular=p0.get_deta_joint_angular(detat,uvlist, z, desiruv, uvlist[0], q_now)

        #             q_pub_now=p0.get_joint_angular(q_now,detaangular)
        #             print "q_now\n", q_now
        #             print "q_pub_now\n",q_pub_now
        #             down_to_q=q_pub_now
        #             ss = "movej([" + str(q_pub_now[0]) + "," + str(q_pub_now[1]) + "," + str(q_pub_now[2]) + "," + str(
        #                 q_pub_now[3]) + "," + str(q_pub_now[4]) + "," + str(q_pub_now[5]) + "]," + "a=" + str(ace) + "," + "v=" + str(
        #                 vel) + "," + "t=" + str(urt) + ")"
        #             # print ss
        #             ur_pub.publish(ss)
        #         if p0.return_error_ok_desire(feature_error.tolist()[0][0],feature_error.tolist()[0][1])==True:
        #             depth=-0.15
        #             temp_q=p0.get_inverse_to_box(down_to_q,0,0,depth)
        #             p0.move_ur_l(ur_pub,temp_q,0.2,ace,urt)
        #             time.sleep(1)
        #             if place_count>=4:
        #                 pose_place=p0.caculate_place_pose_from_ref(temp_q,0)
        #             else:
        #                 pose_place=p0.caculate_place_pose_from_ref(temp_q,1)
        #             rospy.set_param("/open_go_to_object",0)
        #             p0.move_ur_l(ur_pub,pose_place[0],0.2,ace,urt)
        #             time.sleep(1)
        #             p0.move_ur_l(ur_pub,pose_place[1],0.2,ace,urt)
        #             time.sleep(1)
        #             p0.move_ur_l(ur_pub,pose_place[place_count],0.2,ace,urt)
        #             time.sleep(1)

        #             temp_q_place=p0.get_inverse_to_box(pose_place[place_count],0,0,-0.44)
        #             p0.move_ur_l(ur_pub,temp_q_place,0.2,ace,urt)
        #             time.sleep(1)
        #             os.system("rostopic pub /io_state std_msgs/String '55C8010055' -1")

        #             # p0.move_ur_l(ur_pub,cartisian_feedback,0.2,ace,urt)#for debug
        #             p0.move_ur_l(ur_pub,pose_place[place_count],0.2,ace,urt)
        #             time.sleep(2)

        #             p0.uv_desire_list_buffer=[]
        #             rospy.set_param("open_go_desire_flag",0)
        #             p0.move_ur_l(ur_pub,start_angular_back,0.2,ace,urt)
        #             time.sleep(5)
        #             rospy.set_param("/open_go_to_desire",0)

        #             rospy.set_param("/open_go_to_object",1)
        #             rospy.set_param("open_ibvs_flag",1)
        #             place_count+=1

        rate.sleep()
Пример #35
0
def main():
    rospy.init_node('odom_reader', anonymous=True)
    rospack = rospkg.RosPack()  # Find rospackge locations
    listener = tf.TransformListener()

    with open(rospack.get_path('map_reader') + '/mapCoords.yaml', 'r') as f:
        doc = yaml.load(f)
    f.close()

    arenaPnt1 = doc["arenaTopLeft"]
    arenaPnt2 = doc["arenaBotRight"]

    rospy.set_param("/arenaPnt1", arenaPnt1)
    rospy.set_param("/arenaPnt2", arenaPnt2)

    badSquare1 = doc["deadZoneTopLeft"]
    badSquare2 = doc["deadZoneBotRight"]

    rospy.set_param("/deadZone1", badSquare1)
    rospy.set_param("/deadZone2", badSquare2)

    rospy.set_param("/currentRobotR", 0.0)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans, rotate) = listener.lookupTransform('/odom', '/base_link',
                                                       rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException):
            continue

        euler = tf.transformations.euler_from_quaternion(rotate)

        roboX = trans[0]
        roboY = trans[1]
        roboR = euler[2]
        print roboX
        print roboY
        print roboR

        rospy.set_param("/currentRobotX", roboX)
        rospy.set_param("/currentRobotY", roboY)
        rospy.set_param("/currentRobotR", roboR)

        rate.sleep()
    def run_single_evaluation(self, target_dir):
        rospy.loginfo("Starting evaluation on target '%s'.", target_dir)
        # Check target dir is valid (approximately)
        if not os.path.isfile(os.path.join(target_dir, "data_log.txt")):
            rospy.logerr(
                "Invalid target directory: Could not find a 'data_log.txt' file."
            )
            return

        # Check for rosbag renaming
        self.eval_log_file = open(os.path.join(target_dir, "data_log.txt"),
                                  'a+')
        lines = [line.rstrip('\n') for line in self.eval_log_file]
        if not "[FLAG] Rosbag renamed" in lines:
            for line in lines:
                if line[:14] == "[FLAG] Rosbag:":
                    file_name = os.path.join(os.path.dirname(target_dir),
                                             "tmp_bags", line[15:] + ".bag")
                    if os.path.isfile(file_name):
                        os.rename(
                            file_name,
                            os.path.join(target_dir, "visualization.bag"))
                        self.writelog(
                            "Moved the tmp rosbag into 'visualization.bag'")
                        self.eval_log_file.write("[FLAG] Rosbag renamed\n")
                    else:
                        self.writelog("Error: unable to locate '" + file_name +
                                      "'.")
                        rospy.logwarn("Error: unable to locate '" + file_name +
                                      "'.")

        self.eval_log_file.close()  # Make it available for voxblox node

        # Create meshes and voxblox eval
        if self.create_meshes:
            # Configure directory
            if not os.path.isdir(os.path.join(target_dir, "meshes")):
                os.mkdir(os.path.join(target_dir, "meshes"))

        if self.evaluate or self.create_meshes or self.evaluate_volume:
            # Set params and call the voxblox evaluator
            rospy.set_param(self.ns_voxblox + "/target_directory", target_dir)
            try:
                self.eval_voxblox_srv()
            except:
                rospy.logerr(
                    "eval_voxblox service call failed. Shutting down.")
                sys.exit(-1)

        # Reopen logfile
        self.eval_log_file = open(os.path.join(target_dir, "data_log.txt"),
                                  'a+')

        if self.create_plots:
            # Create dirs
            if not os.path.isdir(os.path.join(target_dir, "graphs")):
                os.mkdir(os.path.join(target_dir, "graphs"))

            if os.path.isfile(os.path.join(target_dir, "voxblox_data.csv")):
                # Read voxblox data file
                data_voxblox = self.read_voxblox_data(
                    os.path.join(target_dir, "voxblox_data.csv"))
                if len(data_voxblox['RosTime']) > 1:
                    if 'MeanError' in data_voxblox:
                        self.plot_sim_overview(data_voxblox, target_dir)
                    else:
                        rospy.loginfo(
                            "Unevaluated 'voxblox_data.csv', skipping dependent graphs."
                        )
                else:
                    rospy.loginfo(
                        "Too few entries in 'voxblox_data.csv', skipping dependent graphs."
                    )
            else:
                rospy.loginfo(
                    "No 'voxblox_data.csv' found, skipping dependent graphs.")

            if os.path.isfile(os.path.join(target_dir, "performance_log.csv")):
                # Read performance data file
                data_perf = {}
                headers = None
                with open(os.path.join(target_dir,
                                       "performance_log.csv")) as infile:
                    reader = csv.reader(infile,
                                        delimiter=',',
                                        quotechar='|',
                                        quoting=csv.QUOTE_MINIMAL)
                    for row in reader:
                        if row[0] == 'RunTime':
                            headers = row
                            for header in headers:
                                data_perf[header] = []
                            continue
                        for i in range(len(row)):
                            data_perf[headers[i]].append(row[i])

                # Create graph
                if len(data_perf['RosTime']) > 1:
                    self.plot_perf_overview(data_perf, target_dir)
                else:
                    rospy.loginfo(
                        "Too few entries in 'performance_log.csv', skipping dependent graphs."
                    )
            else:
                rospy.loginfo(
                    "No 'performance_log.csv' found, skipping dependent graphs."
                )

            if os.path.isfile(os.path.join(target_dir, "error_hist.csv")):
                # Read error data file
                with open(os.path.join(target_dir,
                                       "error_hist.csv")) as infile:
                    reader = csv.reader(infile,
                                        delimiter=',',
                                        quotechar='|',
                                        quoting=csv.QUOTE_MINIMAL)
                    data = np.array(list(reader))
                    data_error_hist = data[1:, 1:].astype(int)

                # Create graph
                self.plot_error_hist(data_error_hist, target_dir)
            else:
                rospy.loginfo(
                    "No 'error_hist.csv' found, skipping dependent graphs.")

            # Finish
            if self.clear_voxblox_maps:
                # Remove all voxblox maps to free up disk space
                shutil.rmtree(os.path.join(target_dir, 'voxblox_maps'),
                              ignore_errors=True)
            self.eval_log_file.close()
Пример #37
0
    def PickAndPlaceImpl(self, req):

        collision = rospy.get_param("/Collision")
        print("Collision was set to: {}".format(collision))

        # if collision detected
        if collision:

            # ---------
            print "Placing Down Object:" + req.object
            print "Moving to {} PLACE APPROACH".format(req.object)
            self.state = STATE.PLACING
            rospy.sleep(.5)
            if self.stop:
                return
            place_pose_offset = copy.deepcopy(self.object_place_poses)
            place_pose_offset = place_pose_offset[req.object]
            place_pose_offset.position.z = place_pose_offset.position.z + 0.2
            self.moveToPose(place_pose_offset)
            rospy.sleep(.5)
            if self.stop:
                return

            # ---------
            print "Moving to {} PLACE".format(req.object)
            self.moveToPose(self.object_place_poses[req.object])
            if self.stop:
                return
            self._gripper.command_position(100.0)
            #self.state = STATE.PLACED
            if self.stop:
                return
            rospy.sleep(1.0)
            if self.stop:
                return

            # ---------
            print "Moving to {} PLACE APPROACH".format(req.object)
            place_pose_offset = copy.deepcopy(self.object_place_poses)
            place_pose_offset = place_pose_offset[req.object]
            place_pose_offset.position.z = place_pose_offset.position.z + 0.2
            self.moveToPose(place_pose_offset)
            if self.stop:
                return
            rospy.sleep(1.0)
            self._gripper.command_position(0.0)
            if self.stop:
                return
            self.state = STATE.PLACED
            #self.state = STATE.IDLE

            rospy.set_param("/Collision", False)

        # otherwise run as normal
        else:
            #-----
            # check if collision happened yet, stop if it did
            collision = rospy.get_param("/Collision")
            if collision:
                rospy.set_param("/Collision", False)
                # self.stop
                return
            #-----
            if self.stop:
                return
            print "Picking UP Object: " + req.object
            # self._limb.set_joint_position_speed(0.2)
            if self.stop:
                return
            self.state = STATE.NEUTRAL

            # ---------
            '''print "Moving to {} APPROACH + Z".format(req.object)
            rospy.sleep(1.0)
            approach_pose_offset = copy.deepcopy(self.object_approach_poses[req.object])
            approach_pose_offset.position.z = approach_pose_offset.position.z + 0.2;
            approach_pose_offset.position.y = approach_pose_offset.position.y + 0.03;
            self.moveToPose(approach_pose_offset)
            if self.stop:
                return
            self._gripper.command_position(100.0)
            self.state = STATE.APPROACHING
            if self.stop:
                return
            rospy.sleep(1.0)
            if self.stop:
                return

            # ---------
            print "Moving to {} APPROACH".format(req.object)
            rospy.sleep(1.0)
            approach_pose_offset = copy.deepcopy(self.object_approach_poses[req.object])
            approach_pose_offset.position.z = approach_pose_offset.position.z + 0.05;
            approach_pose_offset.position.y = approach_pose_offset.position.y + 0.03;
            self.moveToPose(approach_pose_offset)
            if self.stop:
                return
            self.state = STATE.APPROACHING
            if self.stop:
                return
            rospy.sleep(1.0)
            if self.stop:
                return'''

            # ---------
            #-----
            # check if collision happened yet, stop if it did
            collision = rospy.get_param("/Collision")
            print("Collision was set to: {}".format(collision))
            if collision:
                rospy.set_param("/Collision", False)
                # self.stop
                return
            #-----
            print "Moving to {} PICK + Z".format(req.object)
            self.state = STATE.PICKING
            pick_pose_offset = copy.deepcopy(
                self.object_pick_poses[req.object])
            pick_pose_offset.position.z = pick_pose_offset.position.z + 0.2
            pick_pose_offset.position.y = pick_pose_offset.position.y
            self.moveToPose(pick_pose_offset)
            self._gripper.command_position(100.0)
            if self.stop:
                return
            self.state = STATE.PICKED
            if self.stop:
                return
            rospy.sleep(.5)
            if self.stop:
                return

            # ---------
            #-----
            # check if collision happened yet, stop if it did
            collision = rospy.get_param("/Collision")
            print("Collision was set to: {}".format(collision))
            if collision:
                rospy.set_param("/Collision", False)
                # self.stop
                return
            #-----
            if req.object == "sports_ball":
                print "Moving to {} PICK".format(req.object)
                self.state = STATE.PICKING
                pick_pose_offset = copy.deepcopy(
                    self.object_pick_poses[req.object])
                pick_pose_offset.position.z = pick_pose_offset.position.z - 0.06
                pick_pose_offset.position.y = pick_pose_offset.position.y - 0.04
                pick_pose_offset.position.x = pick_pose_offset.position.x + 0.04
                self.moveToPose(pick_pose_offset)
                if self.stop:
                    return
                self._gripper.command_position(0.0)
                self.state = STATE.PICKED
                if self.stop:
                    return
                rospy.sleep(.5)
                if self.stop:
                    return
            else:
                print "Moving to {} PICK".format(req.object)
                self.state = STATE.PICKING
                pick_pose_offset = copy.deepcopy(
                    self.object_pick_poses[req.object])
                pick_pose_offset.position.z = pick_pose_offset.position.z - 0.075
                pick_pose_offset.position.y = pick_pose_offset.position.y - 0.095
                pick_pose_offset.position.x = pick_pose_offset.position.x + 0.01
                self.moveToPose(pick_pose_offset)
                if self.stop:
                    return
                self._gripper.command_position(0.0)
                self.state = STATE.PICKED
                if self.stop:
                    return
                rospy.sleep(.5)
                if self.stop:
                    return

            # ---------
            #-----
            # check if collision happened yet, stop if it did
            collision = rospy.get_param("/Collision")
            print("Collision was set to: {}".format(collision))
            if collision:
                rospy.set_param("/Collision", False)
                # self.stop
                return
            #-----
            print "Moving to {} APPROACH".format(req.object)
            rospy.sleep(.5)
            pick_pose_offset = copy.deepcopy(
                self.object_pick_poses[req.object])
            pick_pose_offset.position.z = pick_pose_offset.position.z + 0.2
            self.moveToPose(pick_pose_offset)
            if self.stop:
                return
            self.state = STATE.APPROACHING
            if self.stop:
                return
            rospy.sleep(.5)
            if self.stop:
                return

            # ---------
            #-----
            # check if collision happened yet, stop if it did
            collision = rospy.get_param("/Collision")
            print("Collision was set to: {}".format(collision))
            if collision:
                rospy.set_param("/Collision", False)
                # self.stop
                return
            #-----
            print "Placing Down Object:" + req.object
            print "Moving to {} PLACE APPROACH".format(req.object)
            self.state = STATE.PLACING
            rospy.sleep(.5)
            if self.stop:
                return
            place_pose_offset = copy.deepcopy(self.object_place_poses)
            place_pose_offset = place_pose_offset[req.object]
            place_pose_offset.position.z = place_pose_offset.position.z + 0.2
            self.moveToPose(place_pose_offset)
            rospy.sleep(.5)
            if self.stop:
                return

            # ---------
            #-----
            # check if collision happened yet, stop if it did
            collision = rospy.get_param("/Collision")
            print("Collision was set to: {}".format(collision))
            if collision:
                rospy.set_param("/Collision", False)
                # self.stop
                return
            #-----
            print "Moving to {} PLACE".format(req.object)
            self.moveToPose(self.object_place_poses[req.object])
            if self.stop:
                return
            self._gripper.command_position(100.0)
            #self.state = STATE.PLACED
            if self.stop:
                return
            rospy.sleep(1.0)
            if self.stop:
                return

            # ---------
            #-----
            # check if collision happened yet, stop if it did
            collision = rospy.get_param("/Collision")
            print("Collision was set to: {}".format(collision))
            if collision:
                rospy.set_param("/Collision", False)
                # self.stop
                return
            #-----
            print "Moving to {} PLACE APPROACH".format(req.object)
            place_pose_offset = copy.deepcopy(self.object_place_poses)
            place_pose_offset = place_pose_offset[req.object]
            place_pose_offset.position.z = place_pose_offset.position.z + 0.2
            self.moveToPose(place_pose_offset)
            if self.stop:
                return
            rospy.sleep(1.0)
            self._gripper.command_position(0.0)
            if self.stop:
                return
            self.state = STATE.PLACED  #checking the link between state.placed and Work
    def execute_cb(self, goal):

        rospy.set_param("/tabletop_segmentation/x_filter_max",
                        goal.table_region.x_filter_max)
        rospy.set_param("/tabletop_segmentation/x_filter_min",
                        goal.table_region.x_filter_min)
        rospy.set_param("/tabletop_segmentation/y_filter_max",
                        goal.table_region.y_filter_max)
        rospy.set_param("/tabletop_segmentation/y_filter_min",
                        goal.table_region.y_filter_min)
        rospy.set_param("/tabletop_segmentation/z_filter_max",
                        goal.table_region.z_filter_max)
        rospy.set_param("/tabletop_segmentation/z_filter_min",
                        goal.table_region.z_filter_min)

        # check that preempt has not been requested by the client
        if self._as.is_preempt_requested():
            rospy.loginfo('%s: Preempted' % self._action_name)
            self._as.set_preempted()
            return False

        ####################################
        # start executing the action steps #
        ####################################

        object_list = self.execution_steps(goal)
        if object_list:
            self._result.clusters_list = object_list
            rospy.loginfo('%s: Succeeded' % self._action_name)
            self._as.set_succeeded(self._result)
        else:
            self._as.set_succeeded(self._result)
Пример #39
0
def shutdown():
    rospy.set_param('/aggregator/inStateMachine', False)
Пример #40
0
def get_keyboard_command(key_msg):
	global keyboard_command
	keyboard_command = key_msg
	# force other nodes to stop moving and wait when there is a keyboard
	if not is_twist_empty(keyboard_command):
		rospy.set_param("KEY", True)
Пример #41
0
 def execute(self, userdata):
     rospy.set_param('/aggregator/inNavigation', self.state)
     return 'succeeded'
class Planning(object):
    def __init__(self):
        self.logPosition = []
        self.logPositionAll = []
        self.flagLogPosition = 0
        self.flagIsLogging = False
        self.indOptimalLast = 10
        self.desireDirDrive = FORWARD # desireDirDrive是为了触发底层换挡
        self.curDirDrive = self.desireDirDrive # curDirDrive是为了协调内部程序,如航向变换
        self.flagIsShifting = 0
        self.distLog = 0
        self.countSingleLane = 0
        self.countLaneFinish = 0
        self.flagLaneFinish = 0
        self.flagMCUIsShifting = 0
        self.flagChangesPre = True
        self.flagLogPointStart = True
        self.pointStart = np.array([0,0])
        self.newStraightPathFlag = False # 准备错距到新的直线碾压路径,用来合并换道规划路径
        self.cloudTaskID = 0 # 云端接收到新任务,ID增1,通过此变量判断是否有新任务,有新任务后,重置相关标志位和地图
        self.mapEndPoint, self.numGPSallPoint = findMapEndPoint(numAllLaneGLB,distBetweenLaneGLB) # (numAllLaneGLB,distOffsetStep=1.5)
        np.save(os.path.join(cur_path,'../log/mapEndPoint.npy'),self.mapEndPoint)
        self.indexCircleMap = 0
        if mapTypeGLB == 'raw':
            self.GPSmap = np.load(os.path.join(cur_path,'../map/rawMap.npy'))
        else:
            if taskTypeGLB == 'csdCircle':
                genCurveMap()
                self.GPSmap = np.load(os.path.join(cur_path,''.join(['../map/curveMap', str(self.indexCircleMap), '.npy'])))
            elif taskTypeGLB == 'csdLine':
                self.GPSmap = genMapPoint(self.mapEndPoint[self.countLaneFinish][0],self.mapEndPoint[self.countLaneFinish][1],self.numGPSallPoint)
        while not flagPositionReceivedGLB:
            print 'is waiting vehState topic'
            if rospy.is_shutdown():
                 sys.exit()
            time.sleep(0.02)
        self.indLastClosest, self.flagClosestPointSearched = commonFcn.findClosestPointInit(recv, self.GPSmap)
        self.indLastPre = self.indLastClosest # 检测是否换地图
        self.dataPlanning = {'xCtrPath':[], 'yCtrPath':[], \
                    'xGlbPath':[], 'yGlbPath':[], \
                    'pathCandidate':np.array([[0,0],]), 'indOptimal':0, \
                    'modeDrive':NORMAL, 'vbrMode':0,'distObst':1000.}
        self.distYClose = 0.
        self.modeDrive = NORMAL
        self.vbrMode = 0
        self.prePoint = []
        self.toCloudStartedFlag = False # 0: 未开始碾压任务; 1: 已按需开始云端任务
        self.toCloudEndTaskFlag = False # 0: 任务未结束; 1: 当前碾压任务已完成

    def calibrate(self):
        # Calibrated Parameters
        global CALIBRT
        flagOpenCalib = rospy.get_param('~GLBflagOpenCalib') # 控制标定量接收程序
        if flagOpenCalib == 1:
            CALIBRT = rospy.get_param('~CALIBRT')
            rospy.set_param('~GLBflagOpenCalib', 0)
    # @timeit
    def publish(self):
        '''
        '''
        PathMsg = PathDisplay()
        PathMsg.xGlbPath = self.dataPlanning['xGlbPath']
        PathMsg.yGlbPath = self.dataPlanning['yGlbPath']
        # different from the usage in std_msgs
        dim = MultiArrayDimension()
        dim.stride = int(self.dataPlanning['indOptimal'])
        dim.size = self.dataPlanning['pathCandidate'].shape[0]

        PathMsg.xLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,0]).flatten()
        PathMsg.xLocPath.layout.dim = [dim]
        PathMsg.yLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,1]).flatten()
        PathMsg.yLocPath.layout.dim = [dim]
        PathMsg.header.stamp = rospy.Time.now()
        basePathPub.publish(PathMsg)

        PlanMsg = PlanOP()
        PlanMsg.prePoint.x = self.prePoint[0]
        PlanMsg.prePoint.y = self.prePoint[1]
        PlanMsg.prePoint.z = 0.
        PlanMsg.xCtrPath = self.dataPlanning['xCtrPath']
        PlanMsg.yCtrPath = self.dataPlanning['yCtrPath']
        PlanMsg.distObst = self.dataPlanning['distObst']
        PlanMsg.desireDirDrive = self.desireDirDrive
        PlanMsg.curDirDrive = self.curDirDrive
        PlanMsg.flagIsShifting = self.flagIsShifting
        PlanMsg.distYClose = self.distYClose
        PlanMsg.modeDrive = self.modeDrive
        PlanMsg.vbrMode = self.vbrMode
        PlanMsg.startedFlag = self.toCloudStartedFlag
        PlanMsg.endTaskFlag = self.toCloudEndTaskFlag
        PlanMsg.header.stamp = rospy.Time.now()
        planPub.publish(PlanMsg)

    @timeit
    def update(self):
        CAN_flagStart_trace = recv['start_trace']
        modeDriveLane, vbrMode = self.changeDirection(recv)
        self.vbrMode = vbrMode
        indClosest, indPre, self.flagClosestPointSearched = commonFcn.findClosestPoint( \
            recv, self.GPSmap, self.indLastClosest, CALIBRT['distPreview'], \
            self.flagClosestPointSearched)
        self.indLastClosest = indClosest
        self.indLastPre = indPre
        self.prePoint = self.GPSmap[indPre,:]
        # 考虑找不到点的情况,此时停止规划
        # if indClosest >= self.GPSmap.shape[0]-1:
        if self.flagClosestPointSearched == False:
            modeDriveSafe = STOP
            rospy.logwarn('no point is available')
        elif modeDriveLane == STOP:
            # 防止到达终点时,无路点可用,导致polyfit等一系列函数报错
            rospy.loginfo('is stoping and pausing planning')
        else:
            # try:
            self.dataPlanning = self.localPlanning(indClosest,indPre,recv)
            modeDriveSafe = self.dataPlanning['modeDrive']
            # except Exception, e:
            #     modeDriveSafe = STOP_SAFE
            #     rospy.logerr('localPlanning run error')
                # print 'repr(e):\t', repr(e)
            # merge the modeDrive output
        # CAN_flagStart_trace = 1
        if modeDriveLane == STOP or CAN_flagStart_trace == False or \
            recv['stopSafeArea'] == True or recv['cloudStartTask'] == False:
            self.modeDrive = STOP
        else:
            if self.curDirDrive == BACK:
                self.modeDrive = modeDriveSafe # 后退无避障功能
            else:
                self.modeDrive = modeDriveSafe #
        # self.estimateDistErr(recv)
#        print 'indClosest, indPre',indClosest, indPre

    def replyCloudServer(self):
        if recv['cloudTaskID'] > self.cloudTaskID:
            # 说明有新任务
            # 响应云端新任务,重置所有相关标志位和地图

        # 更新任务结束标志位
        if self.flagLaneFinish == 1:
            self.toCloudEndTaskFlag = True
        # 更新发送到云端的开始任务标志位
        if self.toCloudEndTaskFlag == True:
            self.toCloudStartedFlag = False # 复位任务开始标志位
        else:
            if self.modeDrive == NORMAL:
                self.toCloudStartedFlag = True

        self.cloudTaskID = recv['cloudTaskID']

    # 检测正在起步
    def checkStart(self,recv):
        position = recv
        point_self = np.asarray([position['x'],position['y']])
        if np.abs(position['x']) < 0.1 and np.abs(position['y']) < 0.1:
            pass
        else:
            if self.flagLogPointStart == True:
                self.pointStart = np.array([position['x'],position['y']])
                self.flagLogPointStart = False
                self.flagChangesPre = True

            distStart  = np.linalg.norm(point_self-self.pointStart)
            # 起步过渡过程结束
            if distStart > 7.0:
                self.flagChangesPre = False

    # 计算实时偏差,采用点到直线距离
    # 先排除初始化过程,未接收到定位数据的情况
    def estimateDistErr(self,recv):
        position = recv
        if np.abs(position['x']) < 0.1 and np.abs(position['y']) < 0.1:
            # is initializing
            distErr = 0.
        else:
            startPoint,endPoint = self.mapEndPoint[self.countLaneFinish][0],self.mapEndPoint[self.countLaneFinish][1]
            polyCoeff = np.polyfit([startPoint[0],endPoint[0]],[startPoint[1],endPoint[1]],1)
            A_Coeff,C_Coeff = polyCoeff
            B_Coeff = -1.
            distErr = (A_Coeff*position['x']+B_Coeff*position['y']+C_Coeff) / np.sqrt(A_Coeff**2.+B_Coeff**2.)
            print('distErr',distErr)
            GLBlogDistErr.append(distErr)
        self.distYClose = distErr

    def changeDirection(self,recv):
        CAN_flagIsShifting = recv['is_shifting']
        shift_stat = recv['shift']
        position = recv
        modeDriveLane = NORMAL
        point_self = np.asarray([position['x'],position['y']])
        point_mapEnd = np.asarray([self.GPSmap[-1,0],self.GPSmap[-1,1]])
        distToEnd = np.linalg.norm(point_self-point_mapEnd)
        # 圆弧路径规划,不断延长地图
        if taskTypeGLB == 'csdCircle':
            if self.indLastPre >= self.GPSmap.shape[0]-2:
                # 延长地图
                self.indexCircleMap += 1
                if self.indexCircleMap > 3:
                    self.indexCircleMap = 3
                else:
                    GPS_all = np.load(os.path.join(cur_path,''.join(['../map/curveMap', str(self.indexCircleMap), '.npy'])))
                    # self.GPSmap = np.concatenate((self.GPSmap[self.indLastClosest:-1], GPS_all), axis=0)
                    self.GPSmap = np.concatenate((self.GPSmap, GPS_all), axis=0)
        # 检测正在起步
        # self.checkStart(recv)
#        if distToEnd < CALIBRT['distIsEnd']:
#            rospy.logwarn("have reach the path end, please stop")
        distIsEnd = CALIBRT['distIsEnd'] # 5  # 正式在工地上使用错距模式
        if self.curDirDrive == BACK:
            distIsEnd = max(1., (CALIBRT['distIsEnd'] - 3.4))
        if distToEnd < distIsEnd and self.flagIsShifting == 0:
            if taskTypeGLB == 'csdCircle':
                self.flagLaneFinish = 1
            else:
                # self.flagIsShifting == 0是为了避免换挡过程地图未切换时一直满足CALIBRT['distIsEnd']
                self.countSingleLane += 1
                claCurSinglwLane = countSingleLane -1  # 以线进行判断

                if claCurSinglwLane // 8 == 1 or claCurSinglwLane // 8==2 or claCurSinglwLane// 8==3 or claCurSinglwLane//8==4 :
                    self.vbrMode = 1

                if self.countSingleLane >= timesSingleLaneGLB: #指示几趟
                    self.countSingleLane = 0
                    self.countLaneFinish += 1
                    self.newStraightPathFlag = True

                if self.countLaneFinish >= numAllLaneGLB:
                    self.flagLaneFinish = 1
                    self.countLaneFinish = numAllLaneGLB-1 # -1的目的:避免后面程序索引溢出

                if self.flagLaneFinish == 0: # 走完全程停车,则不必变换方向和地图
                    self.flagIsShifting = 1 # start shift
                    # 变换期望的换挡方向
                    if self.desireDirDrive == FORWARD:
                        self.desireDirDrive = BACK
                    else:
                        self.desireDirDrive = FORWARD
        #判断是否正在换挡
        if self.flagIsShifting == 1:
            # # 加self.flagIsShifting == 1是为了避免完全由底层控制,但终点时,无法进入这段程序
            flagShiftDone = False  # init
            if vehicle_model.name == 'A60':
                if (self.desireDirDrive == BACK and shift_stat == shift_R) \
                   or (self.desireDirDrive == FORWARD and shift_stat == shift_D) : # 表明换挡完成
                   flagShiftDone = True
            else:
                if CAN_flagIsShifting >= 1: # 0 1; '>=' is for uncertain float
                #     #第一次判断,认为底层响应了换挡信号
                    self.flagMCUIsShifting = 1
                if self.flagMCUIsShifting==1 and CAN_flagIsShifting==False:  # should: True-->0
                    flagShiftDone = True
            if flagShiftDone == True:
                self.flagIsShifting = 0  # 换挡完成后,flagIsShifting恢复成0
                self.flagMCUIsShifting = 0
                # 表明换挡完成
                self.flagLogPointStart = True

                # self.mapEndPoint同时控制车道和方向。
                # 车道由第一个索引控制
                # 第二个索引 0 1互换,则表示起点终点互换,方向切换
                if self.flagLaneFinish == 0: # 走完全程停车,则不必变换方向和地图
                    self.indLastClosest = 0  # 换地图前重新初始化
                    self.curDirDrive = self.desireDirDrive
                    if not mapTypeGLB == 'raw':

                        if self.curDirDrive == FORWARD:
                            self.GPSmap = genMapPoint(self.mapEndPoint[self.countLaneFinish][0],self.mapEndPoint[self.countLaneFinish][1],self.numGPSallPoint)
                        elif self.curDirDrive == BACK:
                            self.GPSmap = genMapPoint(self.mapEndPoint[self.countLaneFinish][1],self.mapEndPoint[self.countLaneFinish][0],self.numGPSallPoint)

        # 记录跟踪实际坐标点
        if self.flagLogPosition == 0:   # 到终点记录一次并保存后,便不再记录
            if self.curDirDrive == BACK:
                self.flagIsLogging = True
                self.logPosition.append(point_self)
            # 保存记录的跟踪坐标点,并清空为再次记录做准备
            if (self.curDirDrive == FORWARD or self.flagLaneFinish == 1) and self.flagIsLogging == True:  # 逻辑是刚走完BACK,马上切换到FORWARD或是终点
                # 此逻辑不足之处在于,必须走完当前路线,才能保存数据
                self.flagIsLogging = False
                self.logPositionAll.append(self.logPosition)
                self.logPosition = []

        # 停车模式判断
        if (self.flagLaneFinish==1) | (self.flagIsShifting==1): # self.flagLaneFinish只有再次初始化才能清除,意味着车到这只有停车模式
            modeDriveLane = STOP
            self.countChangesPre = 0
            print 'tttttttt',self.flagLaneFinish
        # 用于驾校测试,消除停车积分
        if vehicle_model.name == 'A60':
            if shift_stat == shift_P or shift_stat == shift_N:
               modeDriveLane = STOP
        # 保存实际走过的位置点
        # 全程走完时,计算所有线路回程的跟踪误差。去程因存在规划偏移,故无意义
        if self.flagLaneFinish == 1 and self.flagLogPosition == 0:
            self.flagLogPosition = 1
            np.save(os.path.join(cur_path,'../log/logPositionAll.npy'),self.logPositionAll)
        vbrMode = self.vbrMode
        return modeDriveLane,vbrMode

    def checkObstacle(self,pathCandidate):
        #################################################################
        #通过寻找第一个弧线上第一个栅格为0(障碍物)来确定无障碍弧长
        #弧长越长越好
        #################################################################
        if self.curDirDrive == FORWARD:
            OccupancyValueMat = recv['OccupancyValueForward']
        else:
            OccupancyValueMat = recv['OccupancyValueBackward']
        #障碍物检测横向区域,左右各widthMargin*0.1m 。 校园大圈:11;驾校:22;工程车23,左右总宽4.6m,振动碾对角宽6.6m
        widthMargin = int(CALIBRT['widthObstDetect']/0.1)
        limitNumOccupied = 1 #认定为不可通行时占据栅格数目限值

        x_S = pathCandidate[0]
        y_S = pathCandidate[1]
        indRow = np.minimum(299,np.round(np.array(x_S)/0.1).astype(np.int32))  # 校园大圈:284; 驾校:290
        indRow = np.maximum(0,indRow)
        indRowMat = np.tile(indRow,(2*widthMargin+1,1)).transpose()
        indCol = np.minimum(200,100+np.round(np.array(y_S)/0.1).astype(np.int32)) # 负的在右边
        indColTemp = np.arange(-widthMargin,widthMargin+1,1)
        indColTemp1 =  np.tile(indColTemp,(indCol.shape[0],1))
        indColMatTemp = indColTemp1+indCol.reshape(-1,1)
        indColMat = np.minimum(200,indColMatTemp)
        indColMat = np.maximum(0,indColMat)
        awardOccupancyTemp = OccupancyValueMat[indRowMat,indColMat] # x_S行,2*widthMargin+1列
        checkFree = np.where(awardOccupancyTemp>80) # > 60表示有障碍物
        occupiedRowTemp = np.array(checkFree)[0]
        occupiedRow = np.unique(occupiedRowTemp) # 删除数组中由不同列导致的相同行号
        numOccupiedRow = occupiedRow.size
        closedObstaclDist = 1000.  # 初始值无穷大,表示无障碍物
        if numOccupiedRow <= limitNumOccupied:   #无障碍

            awardOccupancy = 1
        else:
#            awardOccupancy = checkFree[0][0]  # 循环行数,可以抽象等价为路径长度
            awardOccupancy = 0
            '''
            障碍物距离计算
            note: awardOccupancyTemp是OccupancyValueMat的子集, 所以不能直接通过它索引x轴距离。
            需按如下方法:
                首先通过occupiedRow找到障碍物在awardOccupancyTemp中的行数,然后根据这个
                行数,映射到indRowMat矩阵的真实物理意义行数。
            '''
            if self.curDirDrive == BACK:
                distOffset = DIST_OBST_OFFSET_BACK
            else:
                distOffset = DIST_OBST_OFFSET_FORWARD
            closedObstaclDist = (indRowMat[np.min(occupiedRow),0])*0.1 - distOffset
        return awardOccupancy,closedObstaclDist

    # @timeit
    def genCandidate(self,d0,df,wayPointLocal):
        '''
        generate local path candidate
        Coordinate:
            vehicle local coordinate
        Parameter:
            wayPointLocal: wayPoint in local coordinate, two row

        '''
        if not taskTypeGLB == 'campus':
            pathCandidate = commonFcn.bezierBased(df, wayPointLocal)
        else:
            pathCandidate = commonFcn.laneBased(d0,df,wayPointLocal)
        # 障碍物检测,输入车辆坐标系序列点
        awardOccupancy,closedObstaclDist = self.checkObstacle(pathCandidate)
        result = [pathCandidate,awardOccupancy,closedObstaclDist]

        return result

    # @timeit
    def localPlanning(self,indClosest,indPre,position):
        global GLBlogDistErr
        global CALIBRT, countCALIBRTGLB
        global CenterGlobalEndGLB
        flagCandidateAllDie = 0
        safeDist2RightEdge = 0.5
        modeDrive = NORMAL

        xCenterGlobal = np.array(self.GPSmap[indClosest:indPre+1,0])
        yCenterGlobal = np.array(self.GPSmap[indClosest:indPre+1,1])
        # 路径终点,采用之前的路点,避免各种拟合函数错误
        distCenterPathVect = np.array([self.GPSmap[indPre,0:2] - \
                                        self.GPSmap[indClosest,0:2]])
        distCenterPath = np.linalg.norm(np.array([distCenterPathVect]))
        # if distCenterPath < max(5., CALIBRT['distIsEnd']):
        #     xCenterGlobal, yCenterGlobal = CenterGlobalEndGLB
        # else:
        #     CenterGlobalEndGLB = np.array([xCenterGlobal, yCenterGlobal])
        # 抽样中心线
        # numSample = xCenterGlobal.size
        # indexSample = np.arange(0,numSample,int(numSample/100)+1)
        # xCenterGlobal = xCenterGlobal[indexSample]
        # yCenterGlobal = yCenterGlobal[indexSample]
        # bezier平滑中心线,结果发现,转弯处偏离过多
        # wayPoint = np.array([xCenterGlobal,yCenterGlobal])
        # xCenterGlobal, yCenterGlobal = commonFcn.bezierSmooth(wayPoint)

        # spline平滑中心线(1)或单纯增加道路点(0),从而可以使全局地图可以更稀疏,加快最近点搜索速度
        # 经验证此种增加数据点的方法失效,另寻差值方法
        # numGenPathPoint = max(100, int(CALIBRT['distPreview'] / 0.2))
        # spl = UnivariateSpline(xCenterGlobal,yCenterGlobal)
        # spl.set_smoothing_factor(0.)
        # xCenterGlobal = np.linspace(xCenterGlobal[0],xCenterGlobal[-1],numGenPathPoint)
        # yCenterGlobal = spl(xCenterGlobal)
        # 估计当前位置横向偏差
        # 原理:在最近点处,利用点斜式构造切线方程,然后用点到直线方程求距离
        xClosest = self.GPSmap[indClosest,0]
        yClosest = self.GPSmap[indClosest,1]
        polyCoeffCenter = np.polyfit(xCenterGlobal,yCenterGlobal,2)
        polyder1CoeffCenter = np.polyder(polyCoeffCenter) #导函数多项式系数
        polyder1Center = np.polyval(polyder1CoeffCenter,xClosest) # 斜率
        polyCoeffLine = [polyder1Center, yClosest-polyder1Center*xClosest] # A C
        point = [position['x'],position['y']]
        self.distYClose = commonFcn.calcDistPoint2Line(polyCoeffLine,point)
        # 判断当前点在中心线左右,决定初始偏差正负
        if indClosest >= self.GPSmap.shape[0]-2: # 避免索引溢出
            S  = 0
        else:
            PA = np.array([xClosest, yClosest])
            PB = np.array(self.GPSmap[indClosest+1,0:2])
            S = commonFcn.checkSidePoint2Line(PA, PB, point)
        self.distYClose *= np.sign(S)
        # 平滑中心线
        # yCenterGlobal = np.polyval(polyCoeffCenter, xCenterGlobal)
        # 先将最近的全局坐标转换为局部坐标
        pointGlobal = np.array([xCenterGlobal, yCenterGlobal])
        wayPointLocal = commonFcn.global_2_local_vect(position,pointGlobal)
        ##########################################################
        #采用map函数代替for循环,构造不同df的 path candidate
        dfMax = 2.   # 依据道路宽度3.5m设置
        dfStep = 0.1  # candidate采样距离偏差
        d0 = self.distYClose # 规划时刻当前位置偏差
        if not taskTypeGLB == 'campus':
            '''
            出山店遇到障碍物直接停车,不规划绕行
            '''
            dfMapTemp = np.array([0])  #不进行路径规划,只有一条candidate
        else:
            dfMapTemp = np.arange(-dfMax-1.,dfMax+dfStep+1.,dfStep)
        dfMap = dfMapTemp.tolist()
        numCandidate = len(dfMap)
        d0Map = [d0]*numCandidate
        wayPointLocalMap = [wayPointLocal]*numCandidate
        candidate = map(self.genCandidate, d0Map,dfMap,wayPointLocalMap) # 为一个多维list
        pathCandidate = [m for m,n,p in candidate] # 仍然是多维list
        awardOccupancy = [n for m,n,p in candidate]
        ObstaclDist = [p for m,n,p in candidate]
        closedObstaclDist = np.min(ObstaclDist)
        # 检查是否可通行
        indHalfOptimal = np.where(np.array(awardOccupancy)>0)[0] # 索引数组
        dimIndHalfOptimal = np.array(indHalfOptimal).size
        defaultCandi = int(len(dfMap)/2) # 默认直接跟踪中线
        xCtrPath = np.array([])
        yCtrPath = np.array([])
        indOptimal = defaultCandi
        print 'safe number', dimIndHalfOptimal
        if dimIndHalfOptimal == 0:
            flagCandidateAllDie = 1     # 有障碍物,不能通行
            xCtrPath = xCenterGlobal
            yCtrPath = yCenterGlobal
        else:
            # 先找出最右的path,作为右边界。也就是最后一位索引path-->dfSafe[-1]
            dfSafe = dfMapTemp[indHalfOptimal] # df_map是list不能用此方法索引
            # 然后找到距离右边界安全距离的path
            #左手坐标系
            # dfCandi = dfSafe[-1]-1.
            # indOptimalTemp = bisect.bisect_left(dfSafe,dfCandi) # numpy.searchsorted()
            # 右手坐标系
            # idea1
            dfCandi = dfSafe[0]+1.
            indOptimalTemp = bisect.bisect_right(dfSafe,dfCandi) # numpy.searchsorted()
#            indOptimalTemp = np.argmin(costAll) #
            # idea2
            indOptimalTemp = int(len(indHalfOptimal)/2)
            indOptimal = indHalfOptimal[indOptimalTemp] #通过上一步索引,反推剩下一半,也即是全局索引
#            indOptimal = defaultCandi  # 直接跟踪中线,只作用于显示
            pointOptimal = pathCandidate[indOptimal]
            xOptimalGlb, yOptimalGlb = commonFcn.local_2_global_vect(position,pointOptimal)
            # select path to control module, according to option that choose local planning or not.
            if vehicle_model.locPlan == True:
                # use local planning path
                xCtrPath = xOptimalGlb
                yCtrPath = yOptimalGlb
            else:
                # use map directly
                xCtrPath = xCenterGlobal
                yCtrPath = yCenterGlobal
        # 模式判断
        # 首先处理大曲率转弯时,缩短预瞄距离
        distPreviewCurv = CALIBRT['distPreview']
        distPreviewReplan = CALIBRT['distPreview']
#        if recv['flagBigCurv'] == True:
#            distPreviewCurv = 9.
        if flagCandidateAllDie == 1:
            if closedObstaclDist < CALIBRT['distObstDetect']:
                print("closedObstaclDist is ", closedObstaclDist, ' m')
    #            rospy.logwarn("due to obstacle, there is no path, please stop")
                modeDrive = STOP_SAFE
            else:
                # 减速,并缩短规划路径,重规划
                # modeDrive = DECELERATE
                modeDrive = NORMAL # CSD
                countCALIBRTGLB += 5
                distPlanLag = -1. # 规划周期内行驶距离
                distPreviewReplan = CALIBRT['distObstDetect'] + distPlanLag
        else:
            # 恢复预瞄距离
            if countCALIBRTGLB > 0:
                countCALIBRTGLB -= 1
                if countCALIBRTGLB == 0:
                    CALIBRT = rospy.get_param('~CALIBRT')
            modeDrive = NORMAL
        CALIBRT['distPreview'] = min(distPreviewCurv, distPreviewReplan, CALIBRT['distPreview'])
        pathCandidate = np.array(pathCandidate)
        result = {'xCtrPath':xCtrPath, 'yCtrPath':yCtrPath, \
                    'xGlbPath':xCenterGlobal, 'yGlbPath':yCenterGlobal, \
                    'pathCandidate':pathCandidate, 'indOptimal':indOptimal, \
                    'modeDrive':modeDrive, 'distObst':closedObstaclDist}
        return result

def getOccupancyGridForward(data):
    recv['OccupancyValueForward'] = np.array(data.data).reshape(data.info.height,data.info.width).T

def getOccupancyGridBackward(data):
    recv['OccupancyValueBackward'] = np.array(data.data).reshape(data.info.height,data.info.width).T

def getCAN(data):
    recv['is_shifting'] = data.is_shifting
    recv['start_trace'] = data.start_trace

def getVehState(data):
    global flagPositionReceivedGLB
    flagPositionReceivedGLB = True
    recv['x'] = data.x
    recv['y'] = data.y
    recv['z'] = data.z
    recv['roll'] = data.roll
    recv['pitch'] = data.pitch
    recv['azimuth'] = data.azimuth

    recv['speed'] = data.speed
    recv['brake_stat'] = data.brake_stat
    recv['shift'] = data.shift

def getSteerAngle(data):
    recv['curvature'] = data.curvature
    recv['flagBigCurv'] = data.flagBigCurv

def getSafeAreaWarn(data):
    recv['stopSafeArea'] = data.stopSafeArea

def getCloudTask(data):
    recv['cloudTaskID'] = data.taskID
    recv['cloudStartTask'] = data.startTask
    recv['cloudPathType'] = data.VPathType
    # recv['cloudFlagPoint'] = data.taskID


if __name__ == '__main__':
    rospy.init_node('local_planning', anonymous = False)
    recv = {'x':0., 'y':0., 'z':0., 'roll':0., 'pitch':0., 'azimuth':0., \
            'speed':0., 'brake_stat':0, 'shift':shift_P, \
            'is_shifting':False, 'start_trace':START_TRACE_INIT, \
            'OccupancyValueForward':np.zeros([300,201]), \
            'OccupancyValueBackward':np.zeros([300,201]), \
            'curvature':0.000001, 'flagBigCurv':False, 'stopSafeArea':False, \
            'cloudTaskID':0, 'cloudStartTask':False, 'cloudPathType':0, 'vbrMode':0}
    # Loading Parameters
    # Constant Parameters
    param_file = cur_path + '/../params/paramLWB.yaml'
    with open(param_file,'r') as f:
        param = yaml.load(f)
    mapTypeGLB = param["map"]["mapType"]
    taskTypeGLB = param["map"]["taskType"]
    if mapTypeGLB == 'raw' or taskTypeGLB == 'csdCircle':
        # 校园正常测试,而不是模拟出山店
        numAllLaneGLB = 1
        distBetweenLaneGLB = 1.  # 可随意设置,不再用到
        timesSingleLaneGLB = 1
    else:
        numAllLaneGLB = param["map"]["numAllLane"]
        distBetweenLaneGLB = param["map"]["distBetweenLane"]
        timesSingleLaneGLB = param["map"]["timesSingleLane"]
    # initialize Calibrated Parameters
    rospy.set_param('~GLBflagOpenCalib', 0) # 控制标定量接收程序,初始化将yaml中的强制覆盖
#    CALIBRT = rospy.get_param('~CALIBRT')
    param_file = cur_path + '/../params/calibParam.yaml'
    with open(param_file,'r') as f:
        param = yaml.load(f)
    CALIBRT = param["local_planning"]["CALIBRT"]

    rospy.Subscriber("vehState", VehState, getVehState)
    rospy.Subscriber("/forward/final_grid", OccupancyGrid, getOccupancyGridForward)
    rospy.Subscriber("/backward/final_grid", OccupancyGrid, getOccupancyGridBackward)
    rospy.Subscriber("/mcu_flag", mcuFlag, getCAN)
    rospy.Subscriber('steerAngleCmd', stm32TX, getSteerAngle)
    rospy.Subscriber('safeArea', PlanOP, getSafeAreaWarn)
    rospy.Subscriber('cloudChatter',cloudTask, getCloudTask)

    basePathPub = rospy.Publisher('basePath', PathDisplay, queue_size =1)
    planPub = rospy.Publisher('planOutput', PlanOP, queue_size =5)

    state = Planning()
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        state.calibrate()
        state.update()
        state.publish()
        rate.sleep()
    np.save(os.path.join(cur_path,'../log/logDistErr.npy'),GLBlogDistErr)
    rospy.spin()    #rospy.spin()作用是当节点停止时让python程序退出,和C++ spin的作用不同
Пример #43
0
    def genBlocksWorld(self):
        """Generates a world of exclusive-owned colored blocks.

        Creates n_agents+1 clusters of objects, each of which is clustered
        by color, position, proximity, or any combination.

        Agent IDs start at 1, and colors are procedurally named as
        'color0', 'color1', etc.
        """
        n_agents = rospy.get_param("blocks_world/n_agents", 3)
        n_objects = rospy.get_param("blocks_world/n_objects", 20)

        # Variables to cluster blocks by
        cluster_vars = rospy.get_param("blocks_world/cluster_vars",
                                       ['color', 'position',
                                        'proximity', 'time'])

        # Distance paramaters
        avatar_rng = rospy.get_param("blocks_world/avatar_rng", [0.4, 0.8])
        cluster_rng = rospy.get_param("blocks_world/cluster_rng", [0.4, 0.8])
        cluster_rad = rospy.get_param("blocks_world/cluster_rad", 0.3)
        object_rng = rospy.get_param("blocks_world/object_rng", [-1.0, 1.0])

        # Color names
        color_names = rospy.get_param("blocks_world/color_names",
                                      ["none", "red", "green", "blue"])
        
        # Avg interaction periods (how frequently objects are interacted with)
        beta_owned = rospy.get_param("blocks_world/beta_owned", 10.0)
        beta_unowned = rospy.get_param("blocks_world/beta_unowned", 1000.0)
        # Store scenario generation time
        t_now = rospy.Time.now()

        # Update color database
        for c_id in range(n_agents + 1):
            if c_id < len(color_names):
                c_name = color_names[c_id]
            else:
                c_name = "color" + str(c_id)
                color_names.append(c_name)
            rospy.set_param("colors/" + c_name, [[0,0,0],[0,0,0]])
        
        # Generate agents and their avatars
        for i in range(n_agents):
            av = Object(id=self.avt_base_id+i, is_avatar=True)
            ag = Agent(id=i+1, avatar_id=av.id)

            dist = r.uniform(*avatar_rng)
            angle = r.uniform(i, i+1) * 2 * math.pi / n_agents
            
            av.position = Point(x=dist*math.cos(angle) + self.home_pos.x,
                                y=dist*math.sin(angle) + self.home_pos.y,
                                z=self.ground_lvl)

            self.agent_db[ag.id] = ag
            self.object_db[av.id] = av

        # Generate cluster centers
        if 'proximity' in cluster_vars:
            # Centers are avatar locations if clustering by proximity
            centers = ([Point(self.home_pos.x,
                              self.home_pos.y,
                              self.ground_lvl)] +
                       [av.position for av in self.object_db.values()])
        else:
            # Otherwise, just cluster based on absolute position
            centers = []
            for i in range(n_agents + 1):
                dist = r.uniform(*cluster_rng)
                angle = r.uniform(i, i+1) * 2 * math.pi / (n_agents + 1)
                
                c = Point(x=dist*math.cos(angle) + self.home_pos.x,
                          y=dist*math.sin(angle) + self.home_pos.y,
                          z=self.ground_lvl)
                centers.append(c)

        # Generate objects within each cluster
        for i in range(n_objects):
            obj = Object(id=self.obj_base_id+i)
            c_id = i % (n_agents + 1) # Cluster ID, with 0 unowned
            
            # Set membership in block category to 1
            obj.categories["block"] = 1.0

            # Generate object locations
            if ('position' in cluster_vars or 'proximity' in cluster_vars):
                # Cluster around centers
                r_offset = r.uniform(0, cluster_rad)
                r_angle = r.uniform(0, 2*math.pi)
                c = centers[c_id]
                obj.position = Point(x=c.x + r_offset * math.cos(r_angle),
                                     y=c.y + r_offset * math.sin(r_angle),
                                     z=c.z)
            else:
                # Uniform distribution if no position or proximity clustering
                obj.position = Point(x=r.uniform(*object_rng),
                                     y=r.uniform(*object_rng),
                                     z=self.ground_lvl)
                
            # Generate object colors
            if 'color' in cluster_vars:
                # Associate cluster with color 
                obj.color = color_names[c_id]
            else:
                # Choose color uniformly at random
                col_id = r.choice(range(n_agents+1))
                obj.color = color_names[col_id]

            # Generate last interaction times according to exp. distribution
            if 'time' in cluster_vars:
                for a_id in range(1, n_agents+1):
                    if a_id == c_id:
                        t_last_action = np.random.exponential(beta_owned)
                    else:
                        t_last_action = np.random.exponential(beta_unowned)
                    t_last_action = rospy.Duration(t_last_action)
                    obj.t_last_actions[a_id] = t_now - t_last_action

            # Assign ownership if not unowned
            if c_id > 0:
                obj.ownership[c_id] = 1.0

            self.object_db[obj.id] = obj
Пример #44
0
 def execute(self, userdata):
     rospy.set_param('/aggregator/inVisionTracking', self.state)
     return 'succeeded'
Пример #45
0
 def setParam(self, name, value):
     rospy.set_param(self.prefix + "/" + name, value)
     self.updateParamsService([name])
Пример #46
0
 def execute(self, userdata):
     rospy.set_param('/aggregator/inStateMachine', self.state)
     return 'succeeded'
Пример #47
0
    '--target-network-update-freq',
    type=int,
    default=10,
    help="freq (in training examples) in which to flush core network to"
    " target network")
parser.add_argument(
    '--target-network-update-coeff',
    type=float,
    default=1.0,
    help="affine coeff for target network update. 0 => no update,"
    " 0.5 => mean of core/target, 1.0 => clobber target completely")
opts = parser.parse_args()
print "OPTS", opts

# push refreshable args to param server
rospy.set_param("/q_table_policy/discount", opts.q_discount)
rospy.set_param("/q_table_policy/learning_rate", opts.q_learning_rate)
rospy.set_param("/q_table_policy/state_normalisation_squash",
                opts.q_state_normalisation_squash)
rospy.set_param("/q_table_policy/summary_log_freq", opts.summary_log_freq)
rospy.set_param("/q_table_policy/target_network_update_freq",
                opts.target_network_update_freq)

# build policy
NUM_ACTIONS = 3  # TODO: shared with sim
if opts.policy == "Baseline":
    policy = policy.baseline.BaselinePolicy()
elif opts.policy == "DiscreteQTablePolicy":
    policy = policy.discrete_q_table.DiscreteQTablePolicy(
        num_actions=NUM_ACTIONS)
elif opts.policy == "NNQTablePolicy":
Пример #48
0
 def setParams(self, params):
     for name, value in params.iteritems():
         rospy.set_param(self.prefix + "/" + name, value)
     self.updateParamsService(params.keys())
Пример #49
0
    def startCloudRecognition(self):
        #Get path to the local cloud data base
        object_path = roslib.packages.get_pkg_dir(
            'qbo_webi'
        ) + "/config/" + "cloud_objects_" + self.language["current_language"]

        #Check if the objects folder exists and if not, create it
        if not os.path.exists(object_path):
            os.makedirs(object_path)

        rospy.loginfo("Qbo Webi: Path for the cloud's objects: " + object_path)

        #If version file does not exists
        if not os.path.isfile(object_path + "/version"):
            #Download latest version
            rospy.loginfo(
                "Qbo Webi: Downloading latest version from the Q.bo Cloud")
            resp = self.download_latest_version(object_path)
            if resp != "ok":
                return json.dumps({
                    'status':
                    'Unable to download latest version from Q.bo Cloud',
                    'object': 'none'
                })

        else:
            #Read version file
            version_str = ""
            try:
                version_file = open(object_path + "/version")
                version_str = version_file.read().strip()
                version_file.close()
            except IOError as e:
                return json.dumps({
                    'status': 'Could not access local files',
                    'object': 'none'
                })

            #Get latest version of Q.bo Cloud
            rospy.loginfo(
                "Qbo Webi: Getting version number from Q.bo Cloud...")
            resp = self.get_version()
            if resp == "":
                return json.dumps({
                    'status': 'Could not get version number from Q.bo Cloud',
                    'object': 'none'
                })

            rospy.loginfo("Qbo Webi: Latest version is " + resp +
                          " and local version is " + version_str)

            if resp == version_str:  #If you have latest version, don't do anything
                rospy.loginfo(
                    "Qbo Webi: Local version is the latest version: " +
                    version_str)
            else:
                rospy.loginfo(
                    "Qbo Webi: Downloading latest version from Q.bo Cloud")
                #download latest version
                resp = self.download_latest_version(object_path)
                if resp != "ok":
                    return json.dumps({
                        'status':
                        'Unable to download latest version from Q.bo Cloud',
                        'object': 'none'
                    })
                rospy.loginfo(
                    "Qbo Webi: Latest version has been succesfully downloaded from Q.bo Cloud"
                )

        #Store the previous update_path
        update_path_back = roslib.packages.get_pkg_dir(
            'qbo_object_recognition') + "/objects/objects_db/"
        rospy.loginfo("Qbo Webi: Storing the previous update path: " +
                      update_path_back)

        #Change the update_path of the object recognizer
        rospy.set_param("/qbo_object_recognition/update_path", object_path)

        rospy.loginfo(
            "Qbo Webi: Loading object recognition with the cloud object path")
        #Update the objects folder of the object recognizer to the latest version of Q.bo cloud

        rospy.loginfo("Qbo Webi: Waiting for service update...")
        rospy.wait_for_service("/qbo_object_recognition/update")
        rospy.loginfo("Qbo Webi: Update service is ready!")
        update_srv = rospy.ServiceProxy('/qbo_object_recognition/update',
                                        Update)
        resultUpdating = update_srv()

        #Call the recognize service of the object recognizer

        rospy.loginfo(
            "Qbo Webi: Started object recognizer mode. Waiting for service...")
        rospy.wait_for_service(
            "/qbo_object_recognition/recognize_with_stabilizer")
        rospy.loginfo("Qbo Webi: Recognize service is ready!")
        recog = rospy.ServiceProxy(
            '/qbo_object_recognition/recognize_with_stabilizer',
            RecognizeObject)
        object_recog = recog()

        #Restore the previous update_path and load the previous object daba base
        rospy.set_param("/qbo_object_recognition/update_path",
                        update_path_back)

        rospy.loginfo(
            "Restoring the object recognition node with the previous data base"
        )
        update_srv = rospy.ServiceProxy('/qbo_object_recognition/update',
                                        Update)
        resultUpdating = update_srv()

        output = json.dumps({
            'status': 'ok',
            'object': str(object_recog.object_name)
        })

        return output
Пример #50
0
            launch.launch(virtual_sensor_node)

        except rospy.exceptions.ROSException:
            print('Object Spawning Error')
            pass

    try:
        launch.spin()
    except rospy.exceptions.ROSInterruptException:
        # After Ctrl+C is pressed.
        pass


if __name__ == '__main__':

    # Specify the initial sensor poses and target poses here.
    # Simplified pose format: [x,y,theta].
    # The number of sensors and targets to use is automatically determined by the dimensions of poses passed in.
    sensor_poses=[
    # [1,1,-3.04],\
    [1,1.5,-3.04],\
    [1,0.5,-3.04]]
    target_poses = [[2.4, 4.5, 3.14 / 2]]

    noise_level = input('Additive Gaussian Noise Std(a non-negative number):')
    rospy.set_param('noise_level', float(noise_level))
    # Specify the path to a basis launch file. It usually contains information about the .world file.
    # Here we use the empty world launch file provided by gazebo_ros.
    launch_simulation(sensor_poses=sensor_poses, target_poses=target_poses)
        #Give time for everything to start up
        if args.less_wait:
            time.sleep(10)
        else:
            if args.debug:
                time.sleep(
                    27
                )  #spawning a bunch of xterms for debugging takes longer than subprocesses
            else:
                time.sleep(17)

    #End If different physical genome

    print('Loading received genome into ros param and setting ready msg')
    # Load the data into a parameter in ROS
    rospy.set_param('rover_genome', data['genome'])

    # Send a ready message on the topic to the behavior node
    pub.publish(std_msgs.msg.Empty())

    print('Done! Entering sleep onto sim evaluation is complete.')

    # Wait for a result to return from the basicbot node
    # TODO: Remove this spinning while loop with a different construct.
    # Dependent upon the internal structure of ROS nodes!
    # TODO: get the dependent software crash reset working wihtout having to send back a -1 result
    this_eval_start = datetime.datetime.now()
    while evaluation_result == '':
        current_time = datetime.datetime.now()
        if (current_time -
                this_eval_start).total_seconds() > max_single_sim_running_time:
    def cbCameraInfo(self):

        self.imgWidth = rospy.get_param("/raspicam/width")
        self.imgHeight = rospy.get_param("/raspicam/height")

        rospy.set_param("/raspicam/vFlip", True)
Пример #53
0
def retrieve(listener,
             br,
             isExecute=True,
             objId='expo_eraser',
             binId=0,
             withPause=False,
             viz_pub=None,
             recorder=None,
             ws_object=None):

    liftoff_pub = rospy.Publisher('/liftoff_time',
                                  std_msgs.msg.String,
                                  queue_size=10,
                                  latch=False)

    def compose_output():
        return {
            'collision': collision,
            'grasp_possible': grasp_possible,
            'plan_possible': plan_possible,
            'execution_possible': execution_possible,
            'gripper_opening': gripper_opening,
            'graspPose': graspPose,
            'gelsight_data': gelsight_data,
            'final_object_pose': final_object_pose
        }

    q_initial = ik.helper.get_joints()
    bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
    delta_vision_pose[3:7] = np.array([0, 1, 0, 0])
    vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
    vision_pos[2] = 0.17786528
    plans = []
    plans_grasping = []
    graspPose = []
    plan_possible = False
    execution_possible = False
    gripper_opening = 0.0
    grasp_possible = True
    gelsight_data = []
    collision = False
    final_object_pose = None
    rospy.set_param('is_record', False)
    rospy.set_param('is_contact', False)

    # ~10. Move to a location above the bin
    plan, qf, plan_possible = generatePlan(q_initial,
                                           vision_pos,
                                           delta_vision_pose[3:7],
                                           tip_hand_transform,
                                           'fastest',
                                           plan_name='retrieve_object')
    if plan_possible:
        plans_grasping.append(plan)
        q_initial = qf
    else:
        return compose_output()

    if isExecute and plan_possible:
        #Publish liftoff time
        liftoff_msgs = std_msgs.msg.String()
        liftoff_msgs.data = 'Liftoff (Robot command)'
        liftoff_pub.publish(liftoff_msgs)

        #Execute non-guarded grasp plan move
        executePlanForward(plans_grasping, withPause)

    #~Check if picking success
    low_threshold = 0.0035
    high_threshold = 0.027
    #shake robot
    rospy.sleep(1.5)
    if gripper.getGripperopening() > high_threshold and isExecute:
        # We initialize drop listener
        ws_object.start_listening_for_drop(bin_num=binId)

        ik.ik.shake()
        rospy.sleep(2.)

        # We stop drop listener
        ws_object.stop_listening_for_drop()

    ### We stop recording
    if recorder is not None:
        recorder.stop_recording(save_action=True)
        lasers.stop(binId)

    if isExecute and plan_possible:
        rospy.sleep(2)
        gripper_opening = gripper.getGripperopening()
        if gripper_opening > high_threshold:
            rospy.loginfo('[Picking] ***************')
            rospy.loginfo('[Picking] Pick Successful (Gripper Opening Test)')
            rospy.loginfo('[Picking] ***************')
            execution_possible = True  #temporary hack
        else:
            rospy.loginfo('[Picking] ***************')
            rospy.loginfo('[Picking] Pick Inconclusive (Gripper Opening Test)')
            rospy.loginfo('[Picking] ***************')
            execution_possible = None

    elif not isExecute:
        execution_possible = plan_possible

    return compose_output()
 def _copy_to_parameter_server(self):
     for param in extract_params(self.type.config_description):
         rospy.set_param(self._prefix + param['name'],
                         self.config[param['name']])
Пример #55
0
 def sliderKpRepVel(self):
     position = self.horizontalSlider_kp_r_vel.sliderPosition()
     rospy.set_param('swarm_node/local_plan/kp_repulsive_vel',
                     (position / 50.0))
     self.lineEdit_kp_r_vel.setText(
         str(rospy.get_param('swarm_node/local_plan/kp_repulsive_vel')))
Пример #56
0
def place(listener,
          br,
          isExecute=True,
          binId=0,
          withPause=False,
          rel_pose=None,
          BoxBody=None,
          place_pose=None,
          viz_pub=None,
          is_drop=True,
          recorder=None):

    #########################################################
    # fhogan and nikhilcd
    # Description:
    #~2017 picking primitive
    #
    #~Usage
    #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
    #
    #~Parameters:
    #
    #~Inputs:
    # 1) objInput: Input from vision or RViz (list of 12, or 7)
    # 2) listener: Input from vision or RViz (list of 12, or 7)
    # 3) br: Input from vision or RViz (list of 12, or 7)
    # 4) isExecute: execute the robot motion if possible or not? (Bool)
    # 5) objId: name of object (str)
    # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
    # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
    # 8) withPause: Pause between robot motions (Bool)
    # 9) rel_pose: object pose relative to link_6
    # 10) BoxBody: Bounding box points resolved in link 6 frame
    # 11) place_pose: desired position of bounding box
    # 12) viz_pub: desired position of bounding box
    #
    #~Output:
    # Dictionary of following keys:
    # 1) grasp_possible: Does the object fit in the hand? (True/False)
    # 2) plan_possible: Has the plan succeeded? (True/False)
    # 3) execution_possible: Is there anything in the grasp? (True/False)
    # 4) gripper_opening: gripper opening after grasp attempt (in meters)
    # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
    # 6) gelsight_data: [] , dummy for now
    #########################################################

    #rospy.logdebug(msg, *args)
    #rospy.loginfo(msg, *args)
    #rospy.logwarn(msg, *args)
    #rospy.logerr(msg, *args)
    #rospy.logfatal(msg, *args)

    ###############################
    ## Pring input variables for ##
    ###############################
    rospy.logdebug('[Picking] withPause %s', withPause)
    rospy.logdebug('[Picking] rel_pose %s', rel_pose)
    rospy.logdebug('[Picking] BoxBody %s', BoxBody)
    rospy.logdebug('[Picking] place_pose %s', place_pose)

    ########################
    ## Initialize values ##
    ########################
    q_initial = ik.helper.get_joints()
    bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
    vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
    plans = []
    plans_grasping = []
    plans_grasping2 = []
    plans_guarded = []
    plans_guarded2 = []
    plans_placing = []
    plans_placing2 = []
    graspPose = []
    plan_possible = False
    execution_possible = False
    gripper_opening = 0.0
    grasp_possible = True
    gelsight_data = []
    collision = False
    final_object_pose = None
    rospy.set_param('is_record', False)
    rospy.set_param('is_contact', False)

    liftoff_pub = rospy.Publisher('/liftoff_time',
                                  std_msgs.msg.String,
                                  queue_size=10,
                                  latch=False)

    def compose_output():
        return {
            'collision': collision,
            'grasp_possible': grasp_possible,
            'plan_possible': plan_possible,
            'execution_possible': execution_possible,
            'gripper_opening': gripper_opening,
            'graspPose': graspPose,
            'gelsight_data': gelsight_data,
            'final_object_pose': final_object_pose
        }

    #########################
    ## Constant parameters ##
    #########################
    UpdistFromBinFast = 0.15  # Margin above object during "move to a location"
    UpdistFromBinGuarded = 0.05  # Margin above object during "move to a location"
    gripperOriHome = [0, 1, 0, 0]
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    vision_pos[2] = 0.17786528

    #############
    ## Placing ##
    #############

    #if primitive called without placing arguments, go to center of bin
    if rel_pose == None:
        drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
        drop_pose[3:7] = gripperOriHome
    else:
        drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody,
                                                  place_pose, viz_pub,
                                                  listener, br)

    #~ Adjust height according to weigth
    if is_drop:
        drop_offset = 0.15
    else:
        drop_offset = 0.025
    #~set drop pose target z position to be bottom of bin
    drop_pose[2] = bin_pose[2] - rospy.get_param('tote/height') + drop_offset

    #~Predrop: go to top middle bin surface fast without guarded move
    predrop_pos = np.array(drop_pose[0:3])
    predrop_pos[2] = bin_pose[2] + 0.05

    ######################
    ##  Generate plans ##
    ######################
    #~move safe to placing bin number
    rospy.logdebug(
        '[Picking] Collision free placing to binId %s and position %s', binId,
        predrop_pos)
    #        if isExecute:
    #            q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True)
    #            if update_command is not None:
    #                update_command.execute()

    #~0.go up to avoid collision with tote walls
    current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3)
    current_pose[2] = current_pose[2] + 0.15
    plan, qf, plan_possible = generatePlan(q_initial,
                                           current_pose[0:3],
                                           current_pose[3:7],
                                           tip_hand_transform,
                                           'faster',
                                           plan_name='go_up')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~0.1.fast motion to bin surface high
    predrop_pos_high = predrop_pos
    predrop_pos_high[2] = current_pose[2]
    plan, qf, plan_possible = generatePlan(q_initial,
                                           predrop_pos_high,
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'faster',
                                           plan_name='go_bin_surface_high')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~1.fast motion to bin surface
    predrop_pos[2] = bin_pose[2] + 0.05
    plan, qf, plan_possible = generatePlan(q_initial,
                                           predrop_pos,
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'faster',
                                           plan_name='go_bin_surface')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~2. Guarded move down slow
    plan, qf, plan_possible = generatePlan(q_initial,
                                           drop_pose[0:3],
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'fast',
                                           guard_on=WeightGuard(binId,
                                                                threshold=100),
                                           backwards_speed='superSaiyan',
                                           plan_name='guarded_drop')
    if plan_possible:
        plans_placing.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~3. open gripper
    grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)')
    plans_placing2.append(grasp_plan)

    #~4. open spatula
    grasp_plan = EvalPlan('spatula.open()')
    plans_placing2.append(grasp_plan)

    #~5. go to predrop_pos
    plan, qf, plan_possible = generatePlan(q_initial,
                                           predrop_pos[0:3],
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='predrop_pos')
    if plan_possible:
        plans_placing2.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~6. go to final pose
    final_pos = bin_pose[0:3]
    final_pos[2] = bin_pose[2] + 0.3
    plan, qf, plan_possible = generatePlan(q_initial,
                                           final_pos,
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='final_pose')
    if plan_possible:
        plans_placing2.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~ Execute plans
    if plan_possible:
        if isExecute:
            executePlanForward(plans, withPause)

            #We start recording now
            if recorder is not None:
                lasers.start(binId)
                recorder.start_recording(
                    action='placing',
                    action_id=str(
                        datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
                    tote_num=binId,
                    frame_rate_ratio=5,
                    image_size=-1)

            rospy.set_param('is_record', True)
            executePlanForward(plans_placing, withPause)
            if rospy.get_param('is_contact'):
                ik.helper.move_cart(dz=0.015)
            executePlanForward(plans_placing2, withPause)
            rospy.set_param('is_record', False)
            if recorder is not None:
                recorder.stop_recording(save_action=True)
                lasers.stop(binId)

        execution_possible = True
        return compose_output()
Пример #57
0
 def sliderKpAtt(self):
     position = self.horizontalSlider_kp_att.sliderPosition()
     rospy.set_param('swarm_node/local_plan/kp_attractive',
                     (position / 50.0))
     self.lineEdit_kp_att.setText(
         str(rospy.get_param('swarm_node/local_plan/kp_attractive')))
Пример #58
0
def grasp(objInput,
          listener,
          br,
          isExecute=True,
          objId='expo_eraser',
          binId=0,
          withPause=False,
          viz_pub=None,
          recorder=None):

    #########################################################
    # fhogan and nikhilcd
    # Description:
    #~2017 picking primitive
    #
    #~Usage
    #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
    #
    #~Parameters:
    #
    #~Inputs:
    # 1) objInput: Input from vision or RViz (list of 12, or 7)
    # 2) listener: Input from vision or RViz (list of 12, or 7)
    # 3) br: Input from vision or RViz (list of 12, or 7)
    # 4) isExecute: execute the robot motion if possible or not? (Bool)
    # 5) objId: name of object (str)
    # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
    # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
    # 8) withPause: Pause between robot motions (Bool)
    # 9) rel_pose: object pose relative to link_6
    # 10) BoxBody: Bounding box points resolved in link 6 frame
    # 11) place_pose: desired position of bounding box
    # 12) viz_pub: desired position of bounding box
    #
    #~Output:
    # Dictionary of following keys:
    # 1) grasp_possible: Does the object fit in the hand? (True/False)
    # 2) plan_possible: Has the plan succeeded? (True/False)
    # 3) execution_possible: Is there anything in the grasp? (True/False)
    # 4) gripper_opening: gripper opening after grasp attempt (in meters)
    # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
    # 6) gelsight_data: [] , dummy for now
    #########################################################

    #rospy.logdebug(msg, *args)
    #rospy.loginfo(msg, *args)
    #rospy.logwarn(msg, *args)
    #rospy.logerr(msg, *args)
    #rospy.logfatal(msg, *args)

    ###############################
    ## Pring input variables for ##
    ###############################
    rospy.logdebug('[Picking] objInput %s', objInput)
    rospy.logdebug('[Picking] objId %s', objId)
    rospy.logdebug('[Picking] withPause %s', withPause)

    ########################
    ## Initialize values ##
    ########################
    q_initial = ik.helper.get_joints()
    bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
    vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
    plans = []
    plans_grasping = []
    plans_grasping2 = []
    plans_guarded = []
    plans_guarded2 = []
    plans_placing = []
    plans_placing2 = []
    graspPose = []
    plan_possible = False
    execution_possible = False
    gripper_opening = 0.0
    grasp_possible = True
    gelsight_data = []
    collision = False
    final_object_pose = None
    rospy.set_param('is_record', False)
    rospy.set_param('is_contact', False)

    def compose_output():
        return {
            'collision': collision,
            'grasp_possible': grasp_possible,
            'plan_possible': plan_possible,
            'execution_possible': execution_possible,
            'gripper_opening': gripper_opening,
            'graspPose': graspPose,
            'gelsight_data': gelsight_data,
            'final_object_pose': final_object_pose
        }

    #########################
    ## Constant parameters ##
    #########################
    UpdistFromBinFast = 0.15  # Margin above object during "move to a location"
    UpdistFromBinGuarded = 0.05  # Margin above object during "move to a location"
    gripperOriHome = [0, 1, 0, 0]
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    vision_pos[2] = 0.17786528

    ###############################
    ## Picking primitive  ##
    ###############################
    #        ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]),  listener, br, False)
    #~Define reference frames
    world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames(
        listener=listener, br=br)

    #~get grasp pose and gripper opening from vision
    if len(objInput) == 12:
        graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12(
            objInput)
        graspPos = graspPos  # + hand_X*0.02*1
    elif len(objInput) == 7:
        graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7(
            objInput, objId, listener, br)

    #frank hack for stationary spatula:
    grasp_width = 0.11
    #~build gripper orientation matrix 3x3
    hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z])
    hand_orient_norm = hand_orient_norm.transpose()

    #~Grasp relocation script
    hand_orient_quat = ik.helper.mat2quat(hand_orient_norm)
    euler = tf.transformations.euler_from_quaternion(hand_orient_quat)
    theta = euler[2] - np.pi
    colFreePose = collisionFree(graspPos,
                                binId=binId,
                                listener=listener,
                                br=br,
                                finger_opening=grasp_width,
                                safety_margin=0.00,
                                theta=theta)
    graspPos[0:2] = colFreePose[0:2]

    #################################
    ## GENERATE PLANS OF PRIMITIVE ##
    #################################.
    #~ Start from home position
    #        if isExecute:
    #            q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True)
    #            scorpion.back()

    #~0. Move to a location above the bin, Rotate gripper to grasp orientation
    pregrasp_targetPosition = graspPos
    pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast
    plan, qf, plan_possible = generatePlan(q_initial,
                                           pregrasp_targetPosition,
                                           hand_orient_quat,
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='go_safe_bin')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~1. Move to to surface of bin
    pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded
    plan, qf, plan_possible = generatePlan(q_initial,
                                           pregrasp_targetPosition,
                                           hand_orient_quat,
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='go_top_bin')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

#~2. Open gripper
    grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width)
    plans.append(grasp_plan)

    #~ 3. Open spatula
    grasp_plan = EvalPlan('spatula.open()')
    plans.append(grasp_plan)

    grasp_targetPosition = deepcopy(graspPos)
    #        rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition)

    #~4. sleep
    sleep_plan = EvalPlan('rospy.sleep(0.2)')
    plans.append(sleep_plan)

    #~5. perform guarded move down
    grasp_targetPosition[2] = bin_pose[2] - rospy.get_param(
        '/tote/height') + 0.02  #~frank hack for safety

    plan, qf, plan_possible = generatePlan(q_initial,
                                           grasp_targetPosition,
                                           hand_orient_quat,
                                           tip_hand_transform,
                                           'faster',
                                           guard_on=WeightGuard(binId,
                                                                threshold=100),
                                           plan_name='guarded_pick')
    if plan_possible:
        plans_guarded.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~7. Close spatula
    grasp_plan = EvalPlan('spatula.close()')
    plans_guarded2.append(grasp_plan)

    #~8. Close gripper
    grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30))
    plans_guarded2.append(grasp_plan)

    #~9. sleep
    sleep_plan = EvalPlan('rospy.sleep(0.6)')
    plans_guarded2.append(sleep_plan)

    #~10. Move to a location above the bin
    # plan, qf, plan_possible = generatePlan(q_initial, vision_pos, delta_vision_pose[3:7], tip_hand_transform, 'fastest', plan_name = 'retrieve_object')
    # if plan_possible:
    #     plans_grasping.append(plan)
    #     q_initial = qf
    # else:
    #     return compose_output()

    ################
    ## EXECUTION ###
    ################

    if isExecute and plan_possible:

        executePlanForward(plans, withPause)

        ###We start recording now
        if recorder is not None:
            lasers.start(binId)
            recorder.start_recording(
                action='grasping',
                action_id=str(
                    datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
                tote_num=binId,
                frame_rate_ratio=5,
                image_size=-1)

        #Execute guarded move
        rospy.set_param('is_record', True)
        executePlanForward(plans_guarded, withPause)
        if rospy.get_param('is_contact'):
            ik.helper.move_cart(dz=0.005)
        executePlanForward(plans_guarded2, withPause)
        rospy.set_param('is_record', False)

    elif not isExecute:
        execution_possible = plan_possible

    return compose_output()
Пример #59
0
 def sliderSafetyRange(self):
     position = self.horizontalSlider_safety_range.sliderPosition()
     rospy.set_param('swarm_node/local_plan/safety_range',
                     (position / 10.0))
     self.lineEdit_safety_range.setText(
         str(rospy.get_param('swarm_node/local_plan/safety_range')))
Пример #60
0
 def sliderKdRep(self):
     position = self.horizontalSlider_kd_rep.sliderPosition()
     rospy.set_param('swarm_node/local_plan/kd_repulsive',
                     (position / 50.0))
     self.lineEdit_kd_rep.setText(
         str(rospy.get_param('swarm_node/local_plan/kd_repulsive')))