Ejemplo n.º 1
0
    def __init__(self):
        serial_port = rospy.get_param('~serial_port', '/dev/ttyUSB0')

        try:
            self.device = BNO055(serial_port=serial_port)
        except:
            rospy.logerr('unable to find IMU at port {}'.format(serial_port))
            sys.exit(-1)

        if not self.device.begin():
            rospy.logerr('unable to initialize IMU at port {}'.format(serial_port))
            sys.exit(-1)

        status = self.device.get_system_status()
        rospy.loginfo('system status is {} {} {} '.format(*status))
        time.sleep(1)
        calibration_status = self.device.get_calibration_status()
        rospy.loginfo('calibration status is {} {} {} {} '.format(*calibration_status))

        self.device.set_external_crystal(True)

        self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=1)
        self.temp_pub = rospy.Publisher('temperature', Temperature, queue_size=1)
        self.frame_id = rospy.get_param('~frame_id', '/base_imu')
        self.seq = 0
        self.reset_msgs()
Ejemplo n.º 2
0
    def __init__(self, node_name):
        ROS2OpenCV2.__init__(self, node_name)
        
        self.match_threshold = rospy.get_param("~match_threshold", 0.7)
        self.find_multiple_targets = rospy.get_param("~find_multiple_targets", False)
        self.n_pyr = rospy.get_param("~n_pyr", 2)
        self.min_template_size = rospy.get_param("~min_template_size", 25)

        self.scale_factor = rospy.get_param("~scale_factor", 1.2)
        self.scale_and_rotate = rospy.get_param("~scale_and_rotate", False)
        
        self.use_depth_for_detection = rospy.get_param("~use_depth_for_detection", False)
        self.fov_width = rospy.get_param("~fov_width", 1.094)
        self.fov_height = rospy.get_param("~fov_height", 1.094)
        self.max_object_size = rospy.get_param("~max_object_size", 0.28)

        # Intialize the detection box
        self.detect_box = None
        
        self.detector_loaded = False
        
        rospy.loginfo("Waiting for video topics to become available...")

        # Wait until the image topics are ready before starting
        rospy.wait_for_message("input_rgb_image", Image)
        
        if self.use_depth_for_detection:
            rospy.wait_for_message("input_depth_image", Image)
            
        rospy.loginfo("Ready.")
Ejemplo n.º 3
0
    def __init__(self):
        rospy.init_node('turtlebot_teleop')
        
        self.l_scale = rospy.get_param('~drive_scale',0.6)
        self.a_scale = rospy.get_param('~turn_scale',0.9)
        self.deadman_button = rospy.get_param('~deadman_button', 0)

        self.cmd = None
        #publish cmd in Twist type to topic /cmd_vel
        cmd_pub = rospy.Publisher('~cmd_vel', Twist)



   #     announce_pub = rospy.Publisher('/turtlebot_teleop/anounce/teleops',String, latch=True)

  #    anounce_pub.publisher(rospy.get_namespace());
        
        rospy.Subscriber("joy", Joy, self.callback)
  #      rate = rospy.Rate(rospy.get_param('~hz', 20))
        rate = rospy.Rate(10)          #10hz
    #keep sending cmd value while not shutdown
        while not rospy.is_shutdown():
            if self.cmd:
                cmd_pub.publish(self.cmd)
            rate.sleep()
 def __init__(self):
     message_class_str = rospy.get_param("~message", 
                                         "jsk_network_tools/FC2OCSLargeData")
     try:
         self.message_class = get_message_class(message_class_str)
     except:
         raise Exception("invalid topic type: %s"%message_class_str)
     self.lock = Lock()
     self.dynamic_reconfigure = Server(SilverhammerHighspeedStreamerConfig, self.dynamicReconfigureCallback)
     self.launched_time = rospy.Time.now()
     self.packet_interval = None
     self.diagnostic_updater = diagnostic_updater.Updater()
     self.diagnostic_updater.setHardwareID("none")
     # register function to publish diagnostic
     self.diagnostic_updater.add("HighspeedStreamer", self.diagnosticCallback)
     self.send_port = rospy.get_param("~to_port", 16484)
     self.send_ip = rospy.get_param("~to_ip", "localhost")
     self.last_send_time = rospy.Time(0)
     self.last_input_received_time = rospy.Time(0)
     self.last_send_time_pub = rospy.Publisher("~last_send_time", Time)
     self.last_input_received_time_pub = rospy.Publisher(
         "~last_input_received_time", Time)
     self.send_num = 0
     self.rate = rospy.get_param("~send_rate", 2)   #2Hz
     self.socket_client = socket(AF_INET, SOCK_DGRAM)
     self.packet_size = rospy.get_param("~packet_size", 1400) # for MTU:=1500
     subscriber_info = subscribersFromMessage(self.message_class())
     self.messages = {}
     self.subscribe(subscriber_info)
     self.counter = 0
     self.send_timer = rospy.Timer(rospy.Duration(1.0 / self.rate),
                                   self.sendTimerCallback)
     self.diagnostic_timer = rospy.Timer(rospy.Duration(1.0 / 10),
                                         self.diagnosticTimerCallback)
    def __init__(self):
        self.target_setpoint = Twist()
        self.landed_setpoint = Twist()
        self.flying = False
        self.taking_off = False
        self.landing = False
        self.transient_height = 0

        self.assisted_takeoff = rospy.get_param("~assisted_takeoff", True)
        self.assisted_landing = rospy.get_param("~assisted_landing", True)
        self.reconnect_on_takeoff = rospy.get_param("~reconnect_on_takeoff",
                                                    False)

        self.goal_sub = rospy.Subscriber("goal", PoseStamped, self.new_goal)
        self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)

        # Services
        self.ts = rospy.Service('takeoff', Empty, self.takeoff)
        self.ls = rospy.Service('land', Empty, self.land)
        self._update_params = rospy.ServiceProxy('update_params', UpdateParams)
        if self.reconnect_on_takeoff:
            self._reconnect = rospy.ServiceProxy('reconnect', Empty)

        # Send position setpoints 30 times per seconds
        self.timer = rospy.Timer(rospy.Duration(1.0/UPDATE_RATE),
                                 self.send_setpoint)
def controller(data):

    if(data.data>0):
      command = data.data
      ### вперед, назад, влево, вправо, поворот, вверх, вниз, медленнее, быстрее
    if(command/100000==1):  # ArDrone 2.0
      if((command%1000)/100>0):  # 
        go=True
        while go==True:
          params1=rospy.get_param("ardrone1_name")
          params2=rospy.get_param("ardrone1_do")
          params3=rospy.get_param("ardrone1_digit")
          fb=params3[params1-1][0]-params3[params1-1][1]
          rl=params3[params1-1][2]-params3  [params1-1][3]
          ud=params3[params1-1][4]-params3[params1-1][5]
          tt=params3[params1-1][6]
          if(abs(fb)>0.0 or abs(rl)>0.0 or abs(ud)>0.0 or abs(tt)>0.0):
            go=True
          else:
            go=False
          pub3=rospy.Publisher('cmd_vel', Twist)
          odom=Twist()
          odom.linear.x=fb
          odom.linear.y=rl
          odom.linear.z=ud
          odom.angular.x=0.0
          odom.angular.y=0.0
          odom.angular.z=tt
          pub3.publish(odom)
          rospy.loginfo("движение!!!!")
        rospy.loginfo("конец движения!!!!")

    elif(command/100000==2):  # iRobot
      rospy.loginfo("iRorot Robert")
Ejemplo n.º 7
0
    def loadMap(self, pointset):

        #pointset=str(sys.argv[1])
        host = rospy.get_param("datacentre_host")
        port = rospy.get_param("datacentre_port")
        client = pymongo.MongoClient(host, port)
        db=client.autonomous_patrolling
        points_db=db["waypoints"]
        available = points_db.find().distinct("meta.pointset")
        
        #print available
        if pointset not in available :
            rospy.logerr("Desired pointset '"+pointset+"' not in datacentre")
            rospy.logerr("Available pointsets: "+str(available))
            raise Exception("Can't find waypoints.")      

        #points = self._get_points(waypoints_name) 
        points = []
        search =  {"meta.pointset": pointset}
        for point in points_db.find(search) :
            a= point["meta"]["name"]
            b = topological_node(a)
            b.edges = point["meta"]["edges"]
            b.waypoint = point["meta"]["waypoint"]
            b._get_coords()
            b._insert_vertices(point["meta"]["vertices"])
            points.append(b)
        return points
Ejemplo n.º 8
0
    def __init__(self):
        super(ErrorNode, self).__init__()
        self.DEBUG = bool(rospy.get_param('~DEBUG', 'False'))
        # set topics
        # self.ref_topic = rospy.get_param('~ref_topic')
        # self.tgt_topic = rospy.get_param('~tgt_topic')
        self.ref_tag = rospy.get_param('~ref_tag')
        self.tgt_tag = rospy.get_param('~tgt_tag')
        self.ref_topic = '/moos/'+self.ref_tag+'/odom'
        self.tgt_topic = '/moos/'+self.tgt_tag+'/odom'
        # self.pub_topic = rospy.get_param('~pub_topic')
        self.pub_topic = '/error_mags/'+self.ref_tag+'_ref/'+self.tgt_tag+'_tgt'

        # set pubs/subs
        self.pub = rospy.Publisher(self.pub_topic, PoseError)

        self.pub_test = rospy.Publisher('/test_error', Float64)

        self.ref_sub = rospy.Subscriber(self.ref_topic, Odometry, self.onRefUpdate)
        self.tgt_sub = rospy.Subscriber(self.tgt_topic, Odometry, self.onTgtUpdate)
        # Data holders
        self.ref_pose = Odometry()
        self.tgt_pose = Odometry()
        self.output = PoseError()

        try:
            self.sync_tol = rospy.get_param('~sync_tol')
        except:
            self.sync_tol = 0.15
    def __init__(self, whicharm, tf_listener = None, wait_for_services = 1): #whicharm is 'right' or 'left'
        
        #gets the robot_prefix from the parameter server. Default is pr2 
        robot_prefix = rospy.get_param('~robot_prefix', '') 
        self.srvroot = '/'+whicharm+'_arm_kinematics/' 

        #If collision_aware_ik is set to 0, then collision-aware IK is disabled 
 	self.perception_running = rospy.get_param('~collision_aware_ik', 1) 

        self._ik_service = rospy.ServiceProxy(self.srvroot+'get_ik', GetPositionIK, True)
        if self.perception_running:
            self._ik_service_with_collision = rospy.ServiceProxy(self.srvroot+'get_constraint_aware_ik', GetConstraintAwarePositionIK, True)

        self._fk_service = rospy.ServiceProxy(self.srvroot+'get_fk', GetPositionFK, True)
        self._query_service = rospy.ServiceProxy(self.srvroot+'get_ik_solver_info', GetKinematicSolverInfo, True)
        self._check_state_validity_service = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity, True)

        #wait for IK/FK/query services and get the joint names and limits 
        if wait_for_services:
            self.check_services_and_get_ik_info()
        
        if tf_listener == None:
            rospy.loginfo("ik_utilities: starting up tf_listener")
            self.tf_listener = tf.TransformListener()
        else:
            self.tf_listener = tf_listener

        self.marker_pub = rospy.Publisher('interpolation_markers', Marker)

        #dictionary for the possible kinematics error codes
        self.error_code_dict = {}  #codes are things like SUCCESS, NO_IK_SOLUTION
        for element in dir(ArmNavigationErrorCodes):
            if element[0].isupper():
                self.error_code_dict[eval('ArmNavigationErrorCodes.'+element)] = element

        #reads the start angles from the parameter server 
        start_angles_list = rospy.get_param('~ik_start_angles', []) 
 		         
        #good additional start angles to try for IK for the PR2, used  
        #if no start angles were provided 
        if start_angles_list == []: 
            self.start_angles_list = [[-0.697, 1.002, 0.021, -0.574, 0.286, -0.095, 1.699], 
                                      [-1.027, 0.996, 0.034, -0.333, -3.541, -0.892, 1.694], 
                                      [0.031, -0.124, -2.105, -1.145, -1.227, -1.191, 2.690], 
                                      [0.410, 0.319, -2.231, -0.839, -2.751, -1.763, 5.494], 
                                      [0.045, 0.859, 0.059, -0.781, -1.579, -0.891, 7.707], 
                                      [0.420, 0.759, 0.014, -1.099, -3.204, -1.907, 8.753], 
                                      [-0.504, 1.297, -1.857, -1.553, -4.453, -1.308, 9.572]] 
        else: 
            self.start_angles_list = start_angles_list 

        if whicharm == 'left':
            for i in range(len(self.start_angles_list)):
                for joint_ind in [0, 2, 4]:
                    self.start_angles_list[i][joint_ind] *= -1.

        #changes the set of ids used to show the arrows every other call
        self.pose_id_set = 0

        rospy.loginfo("ik_utilities: done init")
Ejemplo n.º 10
0
    def __init__(self):

        node_name = rospy.get_name()
        #print "node_name: " + str(node_name)

        rate = rospy.get_param("~rate", 14.429)
        self.history_size = rospy.get_param("~history_size", 5)
        self.thresh = rospy.get_param("~thresh", 0.05)

        try:
            self.input_topic = rospy.get_param("~input_topic")
        except:
            err = 'Please specify an input topic'
            rospy.logerr('Please specify an input topic')
            raise Exception(err)

        try:
            self.output_topic = rospy.get_param("~output_topic")
        except:
            err = 'Please specify an output topic'
            rospy.logerr(err)
            raise Exception(err)

        self.tf = tf.TransformListener()
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(rate)
        self.history = RingBuffer(self.history_size)

        self.parent = self.get_parent(self.input_topic)
Ejemplo n.º 11
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)
Ejemplo n.º 12
0
def talker():
    rospy.init_node('random_motors', anonymous=False)
    r = rospy.Rate(0.5) # 10hz
    joints = [rospy.get_param("~pan_name", 'pan_joint'), rospy.get_param("~tilt_name", 'tilt_joint')]
    torque_enable = dict()
    servo_position = dict()

    #Configure joints
    for controller in sorted(joints):
        try:
            rospy.loginfo("controller is "+controller)

            # Torque enable/disable control for each servo
            torque_service = '/' + controller + '/torque_enable'
            rospy.wait_for_service(torque_service)
            torque_enable[controller] = rospy.ServiceProxy(torque_service, TorqueEnable)

            # Start each servo in the disabled state so we can move them by hand
            torque_enable[controller](False)

            # The position controllers
            servo_position[controller] = rospy.Publisher('/' + controller + '/command', Float64)
        except:
            rospy.logerr("Can't contact servo services!")

    while not rospy.is_shutdown():
        for controller in sorted(joints):
            rand_val = uniform(-2,2)
            #Send random float to each joint
            servo_position[controller].publish(rand_val)
        r.sleep()
    """
 def __init__(self):
     '''
     Get file locations from parameter server (or use defaults) and setup dictionary
     which specifies which values are updated. 
     '''
     rospy.init_node(NODE)
     print "==> started " + NODE
     
     # get file names from parameter server
     self.file_urdf_in =             rospy.get_param('~urdf_in',        DEFAULT_CALIB_URDF_XACRO_IN)
     self.file_urdf_out =            rospy.get_param('~urdf_out',       DEFAULT_CALIB_URDF_XACRO_OUT)
     self.file_yaml_calib_system =   rospy.get_param('~calib_system',   DEFAULT_YAML_CALIB_SYSTEM)
     self.file_yaml_init_system =    rospy.get_param('~initial_system', DEFAULT_YAML_INITIAL_SYSTEM)
     self.debug =                    rospy.get_param('~debug',          ENABLE_DEBUG_OUTPUT)
     
     # tfs2update stores the transform names [which need to be converted to (x, y, z, roll, pitch, yaw) 
     # and updated in the urdf] and their corresponding property name prefixes as used in calibration.urdf.xarco
     self.tfs2update = {'arm_0_joint':                        'arm_', 
                        'torso_0_joint':                      'torso_',  
                        'head_color_camera_l_joint':          'cam_l_', 
                        #'head_color_camera_r_joint':          'cam_r_', 
                        'head_cam3d_rgb_optical_frame_joint': 'cam3d_'}
     
     # chains2process stores the dh chain names of chains which need to be updated and their corresponding
     # property names/segments as used in calibration.urdf.xarco
     self.chains2update = {'arm_chain':   ['arm_1_ref',
                                           'arm_2_ref',
                                           'arm_3_ref',
                                           'arm_4_ref',
                                           'arm_5_ref',
                                           'arm_6_ref',
                                           'arm_7_ref'], 
                           'torso_chain': ['torso_lower_neck_tilt_cal_offset',
                                           'torso_pan_cal_offset',
                                           'torso_upper_neck_tilt_cal_offset']}
Ejemplo n.º 14
0
 def _load_interface(self):
     interface_file = resolve_url(rospy.get_param('~interface_url', ''))
     if interface_file:
         rospy.loginfo("interface_url: %s", interface_file)
     try:
         data = read_interface(interface_file) if interface_file else {}
         # set the ignore hosts list
         self._re_ignore_hosts = create_pattern('ignore_hosts', data, interface_file, [])
         # set the sync hosts list
         self._re_sync_hosts = create_pattern('sync_hosts', data, interface_file, [])
         self.__sync_topics_on_demand = False
         if interface_file:
             if 'sync_topics_on_demand' in data:
                 self.__sync_topics_on_demand = data['sync_topics_on_demand']
         elif rospy.has_param('~sync_topics_on_demand'):
             self.__sync_topics_on_demand = rospy.get_param('~sync_topics_on_demand')
         rospy.loginfo("sync_topics_on_demand: %s", self.__sync_topics_on_demand)
         self.__resync_on_reconnect = rospy.get_param('~resync_on_reconnect', True)
         rospy.loginfo("resync_on_reconnect: %s", self.__resync_on_reconnect)
         self.__resync_on_reconnect_timeout = rospy.get_param('~resync_on_reconnect_timeout', 0)
         rospy.loginfo("resync_on_reconnect_timeout: %s", self.__resync_on_reconnect_timeout)
     except:
         import traceback
         # kill the ros node, to notify the user about the error
         rospy.logerr("Error on load interface: %s", traceback.format_exc())
         import os
         import signal
         os.kill(os.getpid(), signal.SIGKILL)
Ejemplo n.º 15
0
    def __init__(self):
        rospy.init_node('receiver')
        PACKAGE = rospy.get_param('~package')
        NAME = rospy.get_param('~message_type')
        TOPIC = rospy.get_param('~topic_name')
        PORT = rospy.get_param('~port_number')
        
        MESSAGE = getattr(__import__(PACKAGE, fromlist=[NAME]), NAME)

        receiver_pub = rospy.Publisher(TOPIC, MESSAGE, queue_size=10)

        try:
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.bind(("", PORT))
            sock.listen(1)
            sock, addr = sock.accept()
        except Exception as e:
            sock.close()
            rospy.loginfo("RECEIVER ERROR:")
            rospy.loginfo(e)
            sys.exit() 

        while True:
            compressed_message = self.recv_msg(sock)
            message = zlib.decompress(compressed_message)
            if not message is None:
                depickled_message = pickle.loads(message)
                topic_message = depickled_message
                receiver_pub.publish(topic_message)
Ejemplo n.º 16
0
    def __init__(self):
       	
	#publishing rate
	self.rate = 30 #30Hz
	
	node_name = 'imu_mpu9150_handler'
        rospy.init_node(node_name)
        rospy.loginfo(rospy.get_caller_id() + 'Initialising {} node'.format(node_name))

        # Get all parameters from config (rosparam)
        address = int(rospy.get_param('imu/address', 0x69))
        gyro_scale = int(rospy.get_param('imu/gyroscope/scale', 250))
        acc_scale = int(rospy.get_param('imu/accelerometer/scale', 2))
	
	# Initialize Madgwick Algorithm
	#self.madgwick = MadgwickAHRS(1/30, 2.7101, 0.2);
	self.quaternion = Madgwick(0.041, self.rate)

	rospy.loginfo('Test {} address'.format(address))
        self.imu = Mpu9150(address, gyro_scale, acc_scale)
	#self.imu = Mpu9250(gyro_scale, acc_scale)
	self.imu.calibrate();
	rospy.loginfo("Compass calibration; move the IMU in random positions during the calibration");
	(self.mag_offset, self.mag_scale) = self.imu.magCalibration();
	rospy.loginfo("Calibration done");
Ejemplo n.º 17
0
def main():
	rospy.init_node('exploration')

	# CONFIG
	# robot
	x, y = rospy.get_param('~start_position', [2.5, 2.5])
	z = rospy.get_param('~flight_height', 1.199)
	simulation.configure('start_position', [x, y, z])
	rot = rospy.get_param('~start_orientation', 0.)
	simulation.configure('start_rotation', rot)
	# world
	w, h = rospy.get_param('~world_size', [10., 10.])
	simulation.configure('world_width', w)
	simulation.configure('world_height', h)
	vsize = rospy.get_param('~voxel_size', .1)
	simulation.configure('voxel_size', vsize)

	# spawn simulation thread
	rospy.loginfo('starting computation of exploration path')
	SimWorker().start()
	# register service
	rospy.loginfo('registering waypoint service')
	s = rospy.Service('get_waypoint', GetWaypoint, handle_get_waypoint)
	# don't leave
	rospy.spin()
Ejemplo n.º 18
0
    def start_recognizer(self):
        rospy.loginfo("Starting recognizer... ")

        self.pipeline = gst.parse_launch(self.launch_config)
        self.asr = self.pipeline.get_by_name('asr')
        self.asr.connect('partial_result', self.asr_partial_result)
        self.asr.connect('result', self.asr_result)
        self.asr.set_property('configured', True)
        self.asr.set_property('dsratio', 1)


        # Configure language model
        if rospy.has_param(self._lm_param):
            lm = rospy.get_param(self._lm_param)
        else:
            rospy.logerr('Recognizer not started. Please specify a language model file.')
            return

        if rospy.has_param(self._dic_param):
            dic = rospy.get_param(self._dic_param)
        else:
            rospy.logerr('Recognizer not started. Please specify a dictionary.')
            return

        self.asr.set_property('lm', lm)
        self.asr.set_property('dict', dic)
        self.bus = self.pipeline.get_bus()
        self.bus.add_signal_watch()
        self.bus_id = self.bus.connect('message::application', self.application_message)
        self.pipeline.set_state(gst.STATE_PLAYING)
        self.started = True
Ejemplo n.º 19
0
	def __init__(self):
		#start node
		rospy.init_node("map_saver")
		rospy.on_shutdown(self.die)

		#get parameters from server
		self.save_dir = rospy.get_param("map_save_dir","map_log/") #default to .ros/map_log
		update_rate = rospy.get_param("map_save_update_rate",120) #update once every 2 min
		#create directory and set clean_open
		self.clean_open=self.create_dir()

		#create dictionaries
		self.edges={} #dictionary of edge_id:edge_obj
		self.nodes={} #dictionary of node_id:node_obj

		#begin listening
		rospy.Subscriber("/node",GVGNode, self.node_handler)
		rospy.Subscriber("/edge",GVGEdgeMsg, self.edge_handler)

		self.updated=False

		#while alive and able to open the file
		self.alive=True
		while(self.clean_open and self.alive):
			if(self.updated):
				self.dump(str(rospy.get_time()))
				self.updated=False
			rospy.sleep(update_rate)
    def __init__(self, use_dummy_transform=False):
        rospy.init_node('star_center_positioning_node')
        if use_dummy_transform:
            self.odom_frame_name = ROBOT_NAME+"_odom_dummy"
        else:
            self.odom_frame_name = ROBOT_NAME+"_odom"

        self.marker_locators = {}
        self.add_marker_locator(MarkerLocator(0,(-6*12*2.54/100.0,0),0))
        self.add_marker_locator(MarkerLocator(1,(0.0,0.0),pi))
        self.add_marker_locator(MarkerLocator(2,(0.0,10*12*2.54/100.0),0))
        self.add_marker_locator(MarkerLocator(3,(-6*12*2.54/100.0,6*12*2.54/100.0),0))
        self.add_marker_locator(MarkerLocator(4,(0.0,6*12*2.54/100.0),pi))

        self.pose_correction = rospy.get_param('~pose_correction',0.0)
        self.phase_offset = rospy.get_param('~phase_offset',0.0)
        self.is_flipped = rospy.get_param('~is_flipped',False)


        srv = Server(STARPoseConfig, self.config_callback)

        self.marker_sub = rospy.Subscriber("ar_pose_marker",
                                           ARMarkers,
                                           self.process_markers)
        self.odom_sub = rospy.Subscriber(ROBOT_NAME+"/odom", Odometry, self.process_odom, queue_size=10)
        self.star_pose_pub = rospy.Publisher(ROBOT_NAME+"/STAR_pose",PoseStamped,queue_size=10)
        self.continuous_pose = rospy.Publisher(ROBOT_NAME+"/STAR_pose_continuous",PoseStamped,queue_size=10)
        self.tf_listener = TransformListener()
        self.tf_broadcaster = TransformBroadcaster()
Ejemplo n.º 21
0
def talker():
    global joy_value
    global last_joy
    rospy.init_node('vrep_ros_teleop')
    sub = rospy.Subscriber('~joy', Joy, joy_cb)
    pub = rospy.Publisher('~twistCommand', Twist, queue_size=1)
    axis_linear = rospy.get_param("~axis_linear",1)
    axis_angular = rospy.get_param("~axis_angular",0)
    scale_linear = rospy.get_param("~scale_linear",5.)
    scale_angular = rospy.get_param("~scale_angular",10.)
    timeout = True
    while not rospy.is_shutdown():
        twist = Twist()
        if (rospy.rostime.get_time() - last_joy) < 0.5: 
            if timeout:
                timeout = False
                rospy.loginfo("Accepting joystick commands")
            twist.linear.x = joy_value.axes[axis_linear] * scale_linear
            twist.angular.z = joy_value.axes[axis_angular] * scale_angular
            if twist.linear.x < 0:
                twist.angular.z = - twist.angular.z
        else:
            if not timeout:
                timeout = True
                rospy.loginfo("Timeout: ignoring joystick commands")
        pub.publish(twist)
        rospy.sleep(0.1)
Ejemplo n.º 22
0
 def execute(self, userdata):
     self.__offset = rospy.get_param('youbot_manipulation_ai_re/misc/angle_offset')
     self.__adjust_arm_threshold = rospy.get_param('youbot_manipulation_ai_re/misc/adjust_arm_threshold')
     self.__image_timeout = rospy.get_param('youbot_manipulation_ai_re/misc/image_timeout')
     self.__wait_for_image_loop_rate = rospy.get_param('youbot_manipulation_ai_re/misc/wait_for_image_loop_rate')
     c_height = userdata.camera_params_in['height']
     msg_joint = copy.deepcopy(userdata.joint_position_in)
     
     while(1):
         t1 = rospy.Time.now().to_sec()
         obj = None
         r = rospy.Rate(self.__wait_for_image_loop_rate)
         while(1):
             obj = self.get_object(userdata.selected_object_name_in)
             t2 = rospy.Time.now().to_sec()
             if t2 - t1 > self.__image_timeout:
                 return 'object lost'  
             if obj != None:
                 break
             r.sleep()
         
         if abs(obj.rrect.centerPoint.y + obj.rrect.height/2.0 - c_height) < self.__adjust_arm_threshold:
             userdata.joint_position_out = copy.deepcopy(msg_joint)
             break
         time.sleep(0.5)
         angle = global_data.ik.get_joint_values()[4] - self.__offset
         #x = -0.001 * (cos(angle) - sin(angle))
         #y = -0.001 * (sin(angle) + cos(angle))
         x = 0.001 * cos(angle)
         y = 0.001 * sin(angle)
         msg_joint['x'] += x
         msg_joint['y'] += y
         global_data.ik.change_pos(msg_joint)
     return 'arm adjusted'
Ejemplo n.º 23
0
	def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
		'''
		Initializes the receiver class. 
		port: The serial port to listen to.
		baudrate: Baud rate for the serial communication
		'''

		self._Counter = 0

		rospy.init_node('arduino')

		port = rospy.get_param("~port", "/dev/ttyUSB0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))

		# subscriptions
		rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
		self._SerialPublisher = rospy.Publisher('serial', String)

		self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
		self._OdometryPublisher = rospy.Publisher("odom", Odometry)

		self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
		self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")
		self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
		
		self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)

		self._State = Arduino.CONTROLLER_RESET_REQUIRED

		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine)
Ejemplo n.º 24
0
    def __init__(self):
        rospy.init_node("max_sonar")
        
        self.sensor = maxSonar() 

        # start channel broadcast using SetupAnalogIn
        rospy.wait_for_service('arbotix/SetupAnalogIn')
        analog_srv = rospy.ServiceProxy('arbotix/SetupAnalogIn', SetupChannel) 
        
        req = SetupChannelRequest()
        req.topic_name = rospy.get_param("~name")
        req.pin = rospy.get_param("~pin")
        req.rate = int(rospy.get_param("~rate",10))
        analog_srv(req)

        # setup a range message to use
        self.msg = Range()
        self.msg.radiation_type = self.sensor.radiation_type
        self.msg.field_of_view = self.sensor.field_of_view
        self.msg.min_range = self.sensor.min_range
        self.msg.max_range = self.sensor.max_range

        # publish/subscribe
        self.pub = rospy.Publisher("sonar_range", Range)
        rospy.Subscriber("arbotix/"+req.topic_name, Analog, self.readingCb)

        rospy.spin()
Ejemplo n.º 25
0
    def __init__(self):

        # Get topic class
        topic_class = None
        rospy.loginfo("Waiting for topic to become available...")
        while not rospy.is_shutdown() and topic_class == None:
            topic_class, topic, _ = rostopic.get_topic_class(rospy.resolve_name('in'))
            rospy.sleep(0.5)

        if hasattr(topic_class, 'header'):
            cb = self.header_cb
        elif topic_class == tf2_msgs.msg.TFMessage:
            cb = self.tf_cb
            self.frames = set(rospy.get_param('~frames'))
            self.frame_prefix = rospy.get_param('~frame_prefix')
        else:
            print("Message type {} doesn't have a header field.".format(topic_class))
            sys.exit(1)

        self.start_time = None
        self.actual_start_time = None
        self.time_offset = None

        self.sub = rospy.Subscriber('in', topic_class, cb)
        self.pub = rospy.Publisher('out', topic_class, queue_size=1)
Ejemplo n.º 26
0
    def __init__(self):
        rospy.init_node("velo_gripper_controller")
        self.speed = rospy.get_param("~speed", 0.5)
        self.torque = rospy.get_param("~torque", 500)
        self.min_opening = rospy.get_param("~min", 0.0)
        self.max_opening = rospy.get_param("~max", 2.0)

        self.controller = 'velo_controller'

        if self.torque > 1023:
           self.torque = 1023
        if self.torque < 0:
           self.torque = 0
        #torque_service = '/' + self.controller + '/torque_enable'
        torque_service = '/' + self.controller + '/set_torque_limit'
        # torque_service = '/' + self.controller + '/set_compliance_punch' # min torque
        # torque_service = '/' + self.controller + '/set_compliance_limit' # max torque
        # limit, punch 0-1023
        rospy.wait_for_service(torque_service)
        #self.my_torque_service = rospy.ServiceProxy
        rospy.wait_for_service(torque_service)
        #self.my_torque_service = rospy.ServiceProxy (torque_service, TorqueEnable)
        self.my_torque_service = rospy.ServiceProxy (torque_service, SetTorqueLimit)
        speed_service = '/' + self.controller + '/set_speed'
        rospy.wait_for_service(speed_service)
        # speed_services.append(rospy.ServiceProxy (speed_service, SetSpeed))
        self.my_speed_service = rospy.ServiceProxy (speed_service, SetSpeed)
        # position in radians
        pub_topic = '/' + self.controller + '/command'
        self.velo_pub = rospy.Publisher(pub_topic, Float64)

        # subscribe to command and then spin
        # Pr2GripperCommand(position, max_effort)
        self.server = actionlib.SimpleActionServer(side_c +"_gripper_controller/gripper_action", Pr2GripperCommandAction, execute_cb=self.commandCb, auto_start=False)
        self.server.start()
Ejemplo n.º 27
0
    def __init__(self):
        self.id = rospy.get_param("~id")
        self.prefix = rospy.get_param("~prefix")
        self.gps = None
        self.state = None
        self.mission = None
        self.battery = None
        self.rel_alt = 0
        self.ready = False
        self.type = Droid.MAV_TYPE.unknown
        self.droid_known_data = {}

        self.goto_once = False

        self.mode = Droid.MODE.wait
        self.mode_wait = Droid.MAV_MODE.guided
        self.mode_data = {}

        # Comunication Topic
        self.topic_comm_pub = rospy.Publisher( Droid.TOPIC_COMM, DroidInfo, queue_size=10 )
        rospy.Subscriber( Droid.TOPIC_COMM, DroidInfo, self.handler_topic_comm )

        # Droid services
        rospy.Service( Droid.SERVICE_WAIT, Empty, self.handler_service_wait )
        rospy.Service( Droid.SERVICE_AUTO, Empty, self.handler_service_auto )
        rospy.Service( Droid.SERVICE_FOLLOW, DroidFollow, self.handler_service_follow )
        rospy.Service( Droid.SERVICE_GOTO, DroidGoto, self.handler_service_goto )
        rospy.Service( Droid.SERVICE_MISSION, DroidMission, self.handler_service_mission )

        # Mavros data
        rospy.Subscriber( Droid.TOPIC_GPS, NavSatFix, self.handler_topic_gps )
        rospy.Subscriber( Droid.TOPIC_STATE, State, self.handler_topic_state )
        rospy.Subscriber( Droid.TOPIC_MISSION_STATE, WaypointList, self.handler_topic_mission )
        rospy.Subscriber( Droid.TOPIC_ALT, Float64, self.handler_topic_alt )
        rospy.Subscriber( Droid.TOPIC_BATTERY, BatteryStatus, self.handler_topic_battery )

        # Mavros topic
        self.topic_rc_pub = rospy.Publisher( Droid.TOPIC_RC_OVERRIDE, OverrideRCIn, queue_size = 10 )

        # Mavros service
        rospy.wait_for_service( Droid.SERVICE_MAV_MISSION_GOTO )
        self.service_mav_goto = rospy.ServiceProxy( Droid.SERVICE_MAV_MISSION_GOTO, WaypointGOTO )

        rospy.wait_for_service( Droid.SERVICE_MAV_MISSION_PUSH )
        self.service_mav_mission_push = rospy.ServiceProxy( Droid.SERVICE_MAV_MISSION_PUSH, WaypointPush )

        rospy.wait_for_service( Droid.SERVICE_MAV_MISSION_PULL )
        self.service_mav_mission_pull = rospy.ServiceProxy( Droid.SERVICE_MAV_MISSION_PULL, WaypointPull )

        rospy.wait_for_service( Droid.SERVICE_MAV_MODE )
        self.service_mav_mode = rospy.ServiceProxy( Droid.SERVICE_MAV_MODE, SetMode )

        rospy.wait_for_service( Droid.SERVICE_MAV_ARM )
        self.service_mav_arm = rospy.ServiceProxy( Droid.SERVICE_MAV_ARM, CommandBool )

        rospy.wait_for_service( Droid.SERVICE_MAV_PARAM_SET )
        self.service_mav_param_set = rospy.ServiceProxy( Droid.SERVICE_MAV_PARAM_SET, ParamSet )

        rospy.wait_for_service( Droid.SERVICE_MAV_PARAM_GET )
        self.service_mav_param_get = rospy.ServiceProxy( Droid.SERVICE_MAV_PARAM_GET, ParamGet )
def launch_control_operator_node():
    rospy.init_node('dronemap_control_operator', anonymous=True)
    
    # get parameters for the client
    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')

    # set global parameters for other processes (eg. hearbeat_operator_node)
    rospy.set_param('/cloud_platform_ip_address', platform_ip_address)
    rospy.set_param('/cloud_platform_port_number', platform_port_number)
    rospy.set_param('/buffer_size', buffer_size)
    
    rospy.set_param('/drone_registered', False)
    
    
    # get the formatter
    formatter = create_message_formatter()

    # start the operator
    client = TCPClient("dronemap_control_operator", platform_ip_address, platform_port_number, formatter, buffer_size)
    DronemapControlOperator(client)
    
    # Keep python from exiting until this node is stopped
    rospy.spin()
Ejemplo n.º 29
0
 def save_all_data_params(self):
     """
     Save parameters to data file for future reference.
     """
     experiment_path = rospy.get_param("/faa_experiment/experiment_path")
     params = rospy.get_param("/")
     self.write_yaml_file(os.path.join(experiment_path, "params.yaml"), params)
        def __init__(self):
                rospy.init_node('twist_converter', anonymous=True)
                self.num_robots=int(rospy.get_param('~num_robots',1))
                self.publishers = [None]*self.num_robots; 
                self.subscribers = [None]*self.num_robots; 
                if rospy.has_param('~robot_prefix'): #if there is a robot prefix assume that there is actually one or more
                	#full_param_name = rospy.search_param('robot_prefix')
                	#robot_prefix = rospy.get_param(full_param_name)
                	robot_prefix=rospy.get_param('~robot_prefix')
                	for r in range(self.num_robots):
                		self.publishers[r]=rospy.Publisher(robot_prefix+str(r)+'/cmd_vel',Twist);
                                self.subscribers[r] = rospy.Subscriber(robot_prefix+str(r)+'/image', Image, self.callback, r)

                else: # if no robot prefix, assume that there is only one robot
                	self.publishers[0] = rospy.Publisher('cmd_vel',Twist);rospy.logwarn("assuming /cmd_vel, number of robots actually "
+str(self.num_robots))
                        self.subscribers[0] = rospy.Subscriber("image", Image, self.callback, 0)

                self.data_uri = rospy.get_param("data_uri","/twist");
                self.urls = (self.data_uri,'twist', "/stop","stop","/controller","controller")
                self.data = ['-10']*self.num_robots;
                self.datasize = [0]*self.num_robots;
                self.port=int(rospy.get_param("~port","8080"));
                #self.data_uri2 = rospy.get_param("data_uri","/pose");
                rospy.logwarn("running")
Ejemplo n.º 31
0
 def _post_init(self):
     if not rospy.get_param('~lazy', True):
         self.subscribe()
         self._connection_status = True
Ejemplo n.º 32
0
def load_table():
    table_bounds = map(float, rospy.get_param("table_bounds").split())
    kinbodies.create_box_from_bounds(Globals.pr2.env,
                                     table_bounds,
                                     name="table")
Ejemplo n.º 33
0
 def setup():
     if Globals.pr2 is None:
         Globals.pr2 = PR2.PR2.create(rave_only=args.test)
         if not args.test: load_table()
     if Globals.rviz is None: Globals.rviz = ros_utils.RvizWrapper.create()
     Globals.table_height = rospy.get_param("table_height")
    def __init__(self):
        rospy.init_node('nav_test', anonymous=False)

        rospy.on_shutdown(self.shutdown)

        # How big is the square we want the robot to navigate?
        square_size = rospy.get_param("~square_size", 1.0)  # meters

        # Create a list to hold the waypoint poses
        waypoints = list()

        # Append each of the four waypoints to the list.  Each waypoint
        # is a pose consisting of a position and orientation in the map frame.

        #First Round
        waypoints.append(
            Pose(Point(3.26524591446, 0.372401684523, 0.000),
                 Quaternion(0.000, 0.000, 0.0, 1.0)))
        waypoints.append(
            Pose(Point(4.18248939514, 1.63796591759, 0.000),
                 Quaternion(0.000, 0.000, 0.923879532511, 0.382683432365)))
        waypoints.append(
            Pose(Point(1.63975059986, 2.0675611496, 0.000),
                 Quaternion(0.000, 0.000, -0.707106781187, 0.707106781187)))
        waypoints.append(
            Pose(Point(1.4075371027, 1.04582130909, 0.000),
                 Quaternion(0.000, 0.000, 0.0, 1.0)))

        #Second Round
        waypoints.append(
            Pose(Point(4.18248939514, 1.63796591759, 0.000),
                 Quaternion(0.000, 0.000, 0.923879532511, 0.382683432365)))
        waypoints.append(
            Pose(Point(1.4075371027, 1.04582130909, 0.000),
                 Quaternion(0.000, 0.000, 0.0, 1.0)))

        #Third Round
        waypoints.append(
            Pose(Point(1.63975059986, 2.0675611496, 0.000),
                 Quaternion(0.000, 0.000, -0.707106781187, 0.707106781187)))
        waypoints.append(
            Pose(Point(4.18248939514, 1.63796591759, 0.000),
                 Quaternion(0.000, 0.000, 0.923879532511, 0.382683432365)))
        waypoints.append(
            Pose(Point(3.26524591446, 0.372401684523, 0.000),
                 Quaternion(0.000, 0.000, 0.0, 1.0)))
        waypoints.append(
            Pose(Point(1.4075371027, 1.04582130909, 0.000),
                 Quaternion(0.000, 0.000, 0.0, 1.0)))

        #Last Ending Point
        waypoints.append(
            Pose(Point(0.299762457609, 0.106840103865, 0.000),
                 Quaternion(0.000, 0.000, 0.701216898681, 0.712948007223)))

        #waypoints.append(Pose(Point(1.759, -1.282,0.000), quaternions[2]))
        #waypoints.append(Pose(Point(-0.442, -0.039,  0.000), quaternions[3]))
        #waypoints.append(Pose(Point(1.759, -1.282,0.000), quaternions[4]))

        # Initialize the visualization markers for RViz
        self.init_markers()

        # Set a visualization marker at each waypoint
        for waypoint in waypoints:
            p = Point()
            p = waypoint.position
            self.markers.points.append(p)

        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)

        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base",
                                                      MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))

        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting navigation test")

        # Initialize a counter to track waypoints
        i = 0

        # Cycle through the four waypoints
        while i < 11 and not rospy.is_shutdown():
            # Update the marker display
            self.marker_pub.publish(self.markers)

            # Intialize the waypoint goal
            goal = MoveBaseGoal()

            # Use the map frame to define goal poses
            goal.target_pose.header.frame_id = 'map'

            # Set the time stamp to "now"
            goal.target_pose.header.stamp = rospy.Time.now()

            # Set the goal pose to the i-th waypoint
            goal.target_pose.pose = waypoints[i]

            # Start the robot moving toward the goal
            self.move(goal)

            i += 1
Ejemplo n.º 35
0
#!/usr/bin/python
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
from math import *
from task_manager_lib.TaskClient import *

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

tc.WaitForAuto()
try:
    tc.Wander(front_sector=True)

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))

if not rospy.core.is_shutdown():
    tc.SetManual()

rospy.loginfo("Mission completed")
Ejemplo n.º 36
0
	def __init__(self):
		print "Initializing Launchpad Class"

#######################################################################################################################
		#Sensor variables
		self._Counter = 0

		self._left_encoder_value = 0
		self._right_encoder_value = 0

		self._battery_value = 0
		self._ultrasonic_value = 0

		self._qx = 0
		self._qy = 0
		self._qz = 0
		self._qw = 0

		self._left_wheel_speed_ = 0
		self._right_wheel_speed_ = 0

		self._LastUpdate_Microsec = 0
		self._Second_Since_Last_Update = 0

		self.robot_heading = 0
#######################################################################################################################
		#Get serial port and baud rate of Tiva C Launchpad
		port = rospy.get_param("~port", "/dev/ttyACM0")
		baudRate = int(rospy.get_param("~baudRate", 115200))

#######################################################################################################################
		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))
		#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine)
		rospy.loginfo("Started serial communication")
		

#######################################################################################################################
#Subscribers and Publishers

		#Publisher for left and right wheel encoder values
		self._Left_Encoder = rospy.Publisher('lwheel',Int64,queue_size = 10)		
		self._Right_Encoder = rospy.Publisher('rwheel',Int64,queue_size = 10)		

		#Publisher for Battery level(for upgrade purpose)
		self._Battery_Level = rospy.Publisher('battery_level',Float32,queue_size = 10)
		#Publisher for Ultrasonic distance sensor
		self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',Float32,queue_size = 10)

		#Publisher for IMU rotation quaternion values
		self._qx_ = rospy.Publisher('qx',Float32,queue_size = 10)
		self._qy_ = rospy.Publisher('qy',Float32,queue_size = 10)
		self._qz_ = rospy.Publisher('qz',Float32,queue_size = 10)
		self._qw_ = rospy.Publisher('qw',Float32,queue_size = 10)
	
		#Publisher for entire serial data
		self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)
		
#######################################################################################################################
#Subscribers and Publishers of IMU data topic

		self.frame_id = '/base_link'

	        self.cal_offset = 0.0
        	self.orientation = 0.0
        	self.cal_buffer =[]
        	self.cal_buffer_length = 1000
        	self.imu_data = Imu(header=rospy.Header(frame_id="base_link"))
        	self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
	        self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
        	self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0]
        	self.gyro_measurement_range = 150.0 
        	self.gyro_scale_correction = 1.35
        	self.imu_pub = rospy.Publisher('imu/data', Imu,queue_size = 10)

		self.deltat = 0
		self.lastUpdate = 0

#New addon for computing quaternion
		
		self.pi = 3.14159
		self.GyroMeasError = float(self.pi * ( 40 / 180 ))
		self.beta = float(math.sqrt(3 / 4) * self.GyroMeasError)

		self.GyroMeasDrift = float(self.pi * ( 2 / 180 ))
		self.zeta = float(math.sqrt(3 / 4) * self.GyroMeasDrift)


		self.beta = math.sqrt(3 / 4) * self.GyroMeasError

		self.q = [1,0,0,0]
#######################################################################################################################
#Speed subscriber
		self._left_motor_speed = rospy.Subscriber('left_wheel_speed',Float32,self._Update_Left_Speed)

		self._right_motor_speed = rospy.Subscriber('right_wheel_speed',Float32,self._Update_Right_Speed)
Ejemplo n.º 37
0
    def __init__(self, root):

        rospy.init_node('tello_ui', anonymous=False)

        # rospy.Subscriber('/command_pos', Point, self.command_pos_callback)

        signal.signal(signal.SIGINT, self.onClose)
        signal.signal(signal.SIGTERM, self.onClose)

        try: 
            self.id                = rospy.get_param('~ID')
        except KeyError:
            self.id = 0
        self.publish_prefix = "tello{}/".format(self.id)


        self.point_command_pos = Point(0.0, 0.0, 1.0)
        self.point_command_pos_yaw = 0.0
        self.command_pos = Pose()
        self.rotated_pos = Point()
        self.slam_pos = Point()
        self.twist_manual_control = Twist()
        self.real_world_pos = Point()
        self.delta_pos = Point()
        self.orientation_degree = Point()

        self.allow_slam_control = False

        self.current_mux = 0
        # initialize the root window and image panel
        self.root = root

        self.panel = None
       # self.panel_for_pose_handle_show = None

        # self.client = dynamic_reconfigure.client.Client("orb_slam2_mono")
       

        # create buttons

        self.column = 0
        self.row = 0
        self.frame_column = 0
        self.frame_row = 0

        self.angle_delta_x = 0
        self.angle_delta_y = 0
        self.angle_delta_z = 0
        self.angle = 12.0
        self.angle_radian = self.angle / 180.0 * math.pi

        self.real_world_scale = 3.9636
        self.altitude = 0

        self.init_command_pos_frame_flag = False
        self.init_real_world_frame_flag = False
        self.init_slam_pose_frame_flag = False
        self.init_delta_frame_flag = False
        self.init_speed_frame_flag = False
        self.init_info_frame_flag = False
        self.init_manual_control_frame_flag = False
        self.init_angle_calc_frame_flag = False
        self.init_rotated_frame_flag = False



        self.init_command_pos_frame()

        self.init_real_world_frame()

        self.init_rotated_frame()
        # self.init_slam_pose_frame()

        self.init_delta_frame()

        self.init_speed_frame()

        self.init_info_frame()

        self.init_manual_control_frame()

        # self.init_angle_calc_frame()

        self.init_kd_kp_frame()

        self.update_command_pos_to_gui()





        self.row += 1
        self.column = 0


        # set a callback to handle when the window is closed
        self.root.wm_title("TELLO Controller")
        self.root.wm_protocol("WM_DELETE_WINDOW", self.onClose)

        self.kd = Pose()
        self.kp = Pose()

        rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.slam_callback)
        rospy.Subscriber(self.publish_prefix+'delta_pos', Point, self.delta_pos_callback)
        rospy.Subscriber(self.publish_prefix+'cmd_vel', Twist, self.speed_callback)
        rospy.Subscriber(self.publish_prefix+'flight_data', FlightData, self.flightdata_callback)
        rospy.Subscriber(self.publish_prefix+'allow_slam_control', Bool, self.allow_slam_control_callback)
        rospy.Subscriber(self.publish_prefix+'real_world_scale', Float32, self.real_world_scale_callback)
        rospy.Subscriber(self.publish_prefix+'real_world_pos', PoseStamped, self.real_world_pos_callback) 
        rospy.Subscriber(self.publish_prefix+'rotated_pos', Point, self.rotated_pos_callback)
        rospy.Subscriber(self.publish_prefix+'command_pos', Pose, self.command_pos_callback)
        rospy.Subscriber(self.publish_prefix+'orientation', Point, self.orientation_callback)

        self.command_pos_publisher = rospy.Publisher(self.publish_prefix+'command_pos', Pose, queue_size = 1)
        self.pub_takeoff = rospy.Publisher(self.publish_prefix+'takeoff', Empty, queue_size=1)
        self.pub_land = rospy.Publisher(self.publish_prefix+'land', Empty, queue_size=1)
        self.pub_allow_slam_control = rospy.Publisher(self.publish_prefix+'allow_slam_control', Bool, queue_size=1)
        self.cmd_val_publisher = rospy.Publisher(self.publish_prefix+'cmd_vel', Twist, queue_size = 1)
        self.calibrate_real_world_scale_publisher = rospy.Publisher(self.publish_prefix+'calibrate_real_world_scale', Empty, queue_size = 1)
        self.scan_room_publisher = rospy.Publisher(self.publish_prefix+'scan_room', Bool, queue_size = 1)
        self.kd_publisher = rospy.Publisher(self.publish_prefix+'kd', Pose, queue_size = 1)
        self.kp_publisher = rospy.Publisher(self.publish_prefix+'kp', Pose, queue_size = 1)
        self.kp_publisher = rospy.Publisher(self.publish_prefix+'kp', Pose, queue_size = 1)
        self.pub_mux =  rospy.Publisher('tello_mux', Int32, queue_size = 1)
        

        self.publish_command()
        state_filter.header = header
        state_filter.states = (be,b,om,al,v,a)

        self.fcp.publish(line_coeffs3)
        self.fpp.publish(points_coeffs3_pcl)
        self.sfp.publish(state_filter)

    def was_called(self):
        return self.called


if __name__ == '__main__':
    rospy.init_node('filter')

    ### LOADING PARAMETERS ###
    cmd_vel_topic = rospy.get_param('topics/cmd_vel')
    line_coeffs_topic = rospy.get_param('topics/line_coeffs')
    filtered_line_coeffs_topic = rospy.get_param('topics/filtered_line_coeffs')
    filtered_line_pcl_topic = rospy.get_param('topics/filtered_line_pcl')
    state_filter_topic = rospy.get_param('topics/state_filter')

    base_link = rospy.get_param('tfs/base_link')

    l = rospy.get_param('l')

    verbose = rospy.get_param('kalman/verbose')

    q00 = rospy.get_param('kalman/q00')
    q11 = rospy.get_param('kalman/q11')
    q22 = rospy.get_param('kalman/q22')
    q33 = rospy.get_param('kalman/q33')
    return cont, q_sol.flatten()



if __name__ == "__main__":
    load_file = True
    if load_file:
        dirName = os.path.split(os.path.abspath(os.path.realpath(sys.argv[0])))[0] +  '/casadi_urdf'
        # fileName = "%s/centauro_urdf_6dof_joints_1111110.txt" % dirName
        fileName = "%s/centauro_urdf_7dof_joints_1111110_torso.txt" % dirName
        with open(fileName, 'r') as f:
            urdf = f.read()
            # print urdf
    else:
        urdf = rospy.get_param('robot_description')
    
    kindyn = cas_kin_dyn.CasadiKinDyn(urdf)
    end_effector = 'arm1_8'
    forKin = Function.deserialize(kindyn.fk(end_effector))

    joints = config.JointNames('torso')
    joints.addJoints('arm1')
    # joints = config.JointNames('arm1')
    joint_str = joints.getName()
    joint_num = [1,2,3,4,5,6]
    for joint in joint_str:
        try:
            if int(joint[-1]) not in joint_num:
                joint_str.remove(joint)
        except:
    def __init__(self):
        rospy.init_node('nav_test', anonymous=False)
        
        rospy.on_shutdown(self.shutdown)
        
        # How big is the square we want the robot to navigate?
        square_size = rospy.get_param("~square_size", 1.0) # meters
        
        # Create a list to hold the waypoint poses
        waypoints = list()
        
        # Create the waypoints and add them to the list
        self.createWaypoints()

        # Initialize the visualization markers for RViz
        self.init_markers()
        
        # Set a visualization marker at each waypoint        
        for waypoint in waypoints:           
            p = Point()
            p = waypoint.position
            self.markers.points.append(p)
            
        # Publisher to manually control the robot (e.g. to stop it, queue_size=5)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        
        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
        # Wait 10 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(10))
        
        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting turtlebot navigation")
        
        # Initialize a counter to track waypoints
        i = 0

        # Go through every waypoint and 
        for waypoint in waypoints:
            # Update the marker display
            self.marker_pub.publish(self.markers)

            # Initialize the waypoint goal
            goal = MoveBaseGoal()

            # Use the map frame to define goal poses
            goal.target_pose.header.frame_id = 'map'

            # Set the time stamp to "now"
            goal.target_pose.header.stamp = rospy.Time.now()

            # Set the goal pose to the i-th waypoint
            goal.target_pose.pose = waypoints[i]

            # Start the robot moving toward the goal
            self.move_turtlebot(goal)

            # Increment i to get the next waypoint on the next loop.
            i = i + 1
Ejemplo n.º 41
0
    def __init__(self):
        # 파라미터 설정
        self.lin_vel_joy = rospy.get_param('~lin_vel_joy', 0.69)
        self.ang_vel_joy = rospy.get_param('~ang_vel_joy', 3.67)
        self.camera = rospy.get_param('~camera', 'camera/color/image_raw')
        self.spin_cycle = rospy.Duration(rospy.get_param('~spin_cycle', 0.05))
        self.scale_arrow = rospy.get_param('~scale_arrow', 50)
        self.scale_cross = rospy.get_param('~scale_cross', 30)

        # 화면 초기화
        os.environ['SDL_VIDEO_WINDOW_POS'] = "0, 0"
        pygame.init()
        self.monitor = pygame.display.Info()
        self.width = rospy.get_param('~width',
                                     int(0.48 * self.monitor.current_w))
        self.height = rospy.get_param('~height',
                                      int(0.48 * self.monitor.current_h))
        self.screen = pygame.display.set_mode((self.width, self.height))
        pygame.mouse.set_visible(False)
        pygame.display.set_caption("Shared control interface")

        # 토픽 구독
        print(C_YELLO + '\rInterfacer, BCI 서비스 준비중...' + C_END)
        rospy.Subscriber(self.camera, Image, self.visualize)
        self.color = {
            'data': [
                (255, 223, 36),  # default
                (255, 223, 36),  # M_RIGHT
                (255, 223, 36),  # M_LEFT
                (255, 223, 36),  # M_FORWARD
                (255, 223, 36),
                (255, 223, 36),
                (255, 223, 36),
                (134, 229, 127)
            ],  # M_MOVE
            'time': [rospy.get_time()] * 8
        }

        # 출력 설정
        self.publisher_cmd_intuit = rospy.Publisher('interf/cmd/intuit',
                                                    CmdIntuit,
                                                    queue_size=1)
        self.publisher_cmd_assist = rospy.Publisher('interf/cmd/assist',
                                                    CmdAssist,
                                                    queue_size=1)
        self.publisher_nav_cue = rospy.Publisher('interf/nav_cue',
                                                 NavCue,
                                                 queue_size=1)
        self.publisher_cmd_vel = rospy.Publisher('cmd_vel',
                                                 Twist,
                                                 queue_size=1)
        self.publisher_cmd_joint = rospy.Publisher('cmd_joint',
                                                   JointState,
                                                   queue_size=1)

        # 토픽 구독
        self.cmd = CmdIntuit()
        self.switch_marker = [False, False, False]
        rospy.Subscriber('interf/cmd/intuit', CmdIntuit,
                         self.update_cmd_intuit)
        rospy.Subscriber('interf/robot/motion', RobotMotion,
                         self.update_marker_color)
        rospy.Subscriber('interf/nav_cue', NavCue,
                         self.update_marker_visibility)
        rospy.Subscriber('joy', Joy, self.joystick)
        self.path = []
        rospy.Subscriber('robot/pose', PoseWithCovarianceStamped,
                         self.update_robot_pose)

        # 서비스 시작
        self.publisher_time_start = rospy.Publisher('time/start',
                                                    Time,
                                                    queue_size=1)
        self.publisher_time_end = rospy.Publisher('time/end',
                                                  Time,
                                                  queue_size=1)
        self.time_start = rospy.Time.now()
        self.the_timer = rospy.Timer(rospy.Duration(0.1), self.timer)
        self.path_publisher = rospy.Publisher('interf/path',
                                              MarkerArray,
                                              queue_size=1)
        self.path_visualizer = rospy.Timer(rospy.Duration(0.3),
                                           self.visualize_path)

        rospy.Service('interf/nav2cmd', Nav2Cmd, self.nav2cmd)
        self.key_watcher = rospy.Timer(self.spin_cycle, self.keyboard)
        print(C_YELLO + '\rInterfacer, BCI 서비스 시작' + C_END)
        print(C_GREEN + '\rInterfacer, 초기화 완료' + C_END)
Ejemplo n.º 42
0
    sts = 0
    
    rospy.loginfo("Running Mapper")
    try:
    
        rospy.init_node('mapper')
        
        map_path = ""
        waypoint_file = ""
        
        lm_button_index = -1
        ms_button_index = -1

        if sts == 0:
            if map_path == "":
                map_path = rospy.get_param("~map_path","")
                rospy.loginfo("Found param map_path='%s'" % map_path)
                
            map_path = map_path.strip()
            
            if map_path == "":
                rospy.loginfo("Error: No map_path specified")
                print usage()
                sts = 1
            else:
                rospy.loginfo("Map and waypoints will be saved in: '%s'" % map_path)
                 
        if sts ==0:
            if waypoint_file == "":
                waypoint_file = rospy.get_param("~waypoint_file","")
                rospy.loginfo("Found param Waypointfile='%s'" % waypoint_file)
Ejemplo n.º 43
0
from controller import Robot
from joint_state_publisher import JointStatePublisher
from trajectory_follower import TrajectoryFollower
# from gripper_control import GripperFollower
from rosgraph_msgs.msg import Clock

parser = argparse.ArgumentParser()
parser.add_argument('--node-name',
                    dest='nodeName',
                    default='Panda',
                    help='Specifies the name of the node.')
arguments, unknown = parser.parse_known_args()

rospy.init_node(arguments.nodeName, disable_signals=True)

jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
    print('Setting prefix to %s' % jointPrefix)

# initialPosition = [0, -0.785, 0, -2.356, 0, 1.571, 0.785]

robot = Robot()
nodeName = arguments.nodeName + '/' if arguments.nodeName != 'Panda' else ''
# for i in range(len(initialPosition)):
#     name = 'panda_joint'+str(i+1)
#     robot.getMotor(name).setPosition(initialPosition[i])

jointStatePublisher = JointStatePublisher(robot, jointPrefix, nodeName)
trajectoryFollower = TrajectoryFollower(robot, jointStatePublisher,
                                        jointPrefix, nodeName)
trajectoryFollower.start()
Ejemplo n.º 44
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
Ejemplo n.º 45
0
 def __init__(self, verbose=False):
     self.mc = rospy.get_param("~mc", False)
     self.num_particles = rospy.get_param("~num_particles", 100)
     if verbose:
         print(self)
    def __init__(self):

        self.ERRORS = {0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
                       0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
                       0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
                       0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
                       0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
                       0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"),
                       0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Main batt voltage high"),
                       0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage high"),
                       0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Logic batt voltage low"),
                       0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"),
                       0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"),
                       0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage high"),
                       0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Main batt voltage low"),
                       0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"),
                       0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"),
                       0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
                       0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")}

        rospy.init_node("roboclaw_node_pitch")
        rospy.on_shutdown(self.shutdown)
        rospy.loginfo("Connecting to roboclaw")
        self.dev_name = rospy.get_param("~dev", "/dev/ttyACM3") #may need to change the usb port

        self.baud_rate = int(rospy.get_param("~baud", "38400")) #may need to change the baud rate. see roboclaw usermanual

        self.address = int(rospy.get_param("~address", "128")) #roboclaw is current setup to use address 128 which is setting 7


        #Error Handle
        if self.address > 0x87 or self.address < 0x80:
            rospy.logfatal("Address out of range")
            rospy.signal_shutdown("Address out of range")
        
        # TODO need someway to check if address is correct
        try:
            roboclaw.Open(self.dev_name, self.baud_rate)

        except Exception as e:
            rospy.logfatal("Could not connect to Roboclaw")
            rospy.logdebug(e)
            rospy.signal_shutdown("Could not connect to Roboclaw")

        self.updater = diagnostic_updater.Updater()
        self.updater.setHardwareID("Roboclaw")
        self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals))

        try:
            version = roboclaw.ReadVersion(self.address)
        except Exception as e:
            rospy.logwarn("Problem getting roboclaw version")
            rospy.logdebug(e)
            pass

        if not version[0]:
            rospy.logwarn("Could not get version from roboclaw")
        else:
            rospy.logdebug(repr(version[1]))

        roboclaw.SpeedM1M2(self.address, 0, 0)
        roboclaw.ResetEncoders(self.address)

        self.MAX_SPEED = float(rospy.get_param("~max_speed", "127"))
        self.MAX_DUTY = float(rospy.get_param("~max_duty", "32767"))

        self.last_set_speed_time = rospy.get_rostime()
        self.last_pub_time = rospy.get_rostime()

        self.pitch_vel_pub = rospy.Subscriber("roboclaw/pitch_vel", Twist, self.cmd_vel_callback)
        self.pitch_pub = rospy.Publisher("roboclaw/pitch", JointState, queue_size=10)
        
        self.m1_duty = 0
        self.m2_duty = 0
        
        rospy.sleep(1)

        rospy.loginfo("dev %s", self.dev_name)
        rospy.loginfo("baud %d", self.baud_rate)
        rospy.loginfo("address %d", self.address)
        rospy.loginfo("max_speed %f", self.MAX_SPEED)
Ejemplo n.º 47
0
    def __init__(self):

        # Load a mission and parse the file
        mission_file = rospy.get_param('~mission_file')
        deep_motion_planner = rospy.get_param('~deep_motion_planner',
                                              default=False)
        stage_simulation = rospy.get_param('~stage_simulation', default=True)
        if not os.path.exists(mission_file):
            rospy.logerr('Mission file not found: {}'.format(mission_file))
            exit()

        self.mission = MissionFileParser(mission_file).get_mission()

        self.mission_index = 0  # Currently executed command
        self.random_waypoint_number = 0  # Number of random waypoints remaining
        self.current_target = [0, 0, 0]  # Current goal pose

        self.command_start = rospy.Time.now().to_sec(
        )  # Time when command execution started
        self.command_timeout = rospy.get_param('~command_timeout',
                                               default=360.0)

        self.costmap = None  # Cache for the costmap

        self.tf_broadcaster = tf.TransformBroadcaster()

        # ROS topics
        self.start_pub = rospy.Publisher('/start', Empty, queue_size=1)
        self.stop_pub = rospy.Publisher('/stop', Empty, queue_size=1)
        self.abort_pub = rospy.Publisher('/abort', Empty, queue_size=1)
        self.target_pub = rospy.Publisher('/relative_target',
                                          PoseStamped,
                                          queue_size=1)

        self.costmap_sub = rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, \
                self.__costmap_callback__)
        self.costmap_update_sub = rospy.Subscriber('/move_base/global_costmap/costmap_updates', \
                OccupancyGridUpdate, self.__costmap_update_callback__)
        self.joystick_sub = rospy.Subscriber('/joy', Joy,
                                             self.joystick_callback)

        if deep_motion_planner:
            self.navigation_client = actionlib.SimpleActionClient(
                'deep_move_base', MoveBaseAction)

            while not self.navigation_client.wait_for_server(
                    rospy.Duration(5)):
                rospy.loginfo('Waiting for deep planner action server')

        else:
            # connect to the action api of the move_base package
            self.navigation_client = actionlib.SimpleActionClient(
                'move_base', MoveBaseAction)

            while not self.navigation_client.wait_for_server(
                    rospy.Duration(5)):
                rospy.loginfo('Waiting for move_base action server')

        if stage_simulation:
            rospy.wait_for_service('/reset_positions')

        # Start the mission if it is not empty
        if len(self.mission) > 0:
            rospy.loginfo('Start mission')
            self.__send_next_command__()
        else:
            rospy.logerr('Mission file contains no commands')
            rospy.signal_shutdown('Mission Finished')
Ejemplo n.º 48
0
    def update_cmd_pwm(self, msg):
        pin = int(msg.pin)
        # Gère les messages entre -100 et 100
        if np.abs(msg.command)>100.0:
            msg.command = 0.0
        self.out_tab[pin].publish(msg.command/2.0*10+1500)


if __name__ == '__main__':
    rospy.init_node('simu_pwm_board')

    # === COMMON ===

    # La node doit se lancer en sachant où chercher sa config
    node_name = rospy.get_name()
    device_type_name = rospy.get_param(node_name+'_type_name')

    # Config
    ns = rospy.get_namespace()
    nslen = len(ns)
    prefix_len = nslen + 5 # On enlève le namespace et simu_
    config_node = rospy.get_param('robot/'+device_type_name+'/'+node_name[prefix_len:])
    print config_node

    # Pas sur qu'on en ai besoin mais au cas où
    device_type = config_node['type']
    config_device = rospy.get_param('device_types/'+device_type)
    print config_device

    # === SPECIFIC ===
Ejemplo n.º 49
0
 def loadParameters(self):
     self.droneList = rospy.get_param('~droneList', ['cf2', 'cf3'])
     self.NumDrones = len(self.droneList)
     pass
Ejemplo n.º 50
0
                                 self._right_params.camera_name,
                                 result_msg.right_result)

            rospy.loginfo("Finished Calibrating")
            self._calibrator = None
            #rospy.signal_shutdown('Finished')


if __name__ == '__main__':
    rospy.init_node("stereo_calibrator")
    rospy.loginfo("Loading %s" % rospy.get_name())

    # Get parameters
    left_params = CalibratorParams()
    right_params = CalibratorParams()
    autostart = rospy.get_param("~autostart", False)
    use_service = rospy.get_param("~use_service", True)
    approximate = rospy.get_param("~approximate", 0.0)

    left_params.features = "%s/features" % rospy.remap_name("left")
    left_params.camera_info = "%s/camera_info" % rospy.remap_name("left")
    left_params.service_name = "%s/set_camera_info" % rospy.remap_name(
        "left_camera")
    left_params.camera_name = rospy.get_param("~left_camera_name", "left")
    left_params.file_name = rospy.get_param("~left_file_name", "left.yaml")

    right_params.features = "%s/features" % rospy.remap_name("right")
    right_params.camera_info = "%s/camera_info" % rospy.remap_name("right")
    right_params.service_name = "%s/set_camera_info" % rospy.remap_name(
        "right_camera")
    right_params.camera_name = rospy.get_param("~right_camera_name", "right")
from std_msgs.msg import ColorRGBA

# Shows Center of mass and support polygon in rviz

rospy.init_node('markers_stability', anonymous=False)
pub_markers = rospy.Publisher('/markers/stability',
                              Marker,
                              queue_size=1,
                              latch=True)

rospy.loginfo("Markers Stability started.")
rospy.loginfo("Waiting for COM radius parameter...")
param_name = "/robostilt/safety/center_of_mass_radius"
while (rospy.has_param(param_name) is False):
    rospy.sleep(1.0)
COM_radius = rospy.get_param(param_name)

rospy.loginfo("Markers Stability publishing...")
namespace = "stabilty"
# holds the different markers
indexes = ("com", "com_projected", "support_polygon")

markers = []
# initialize markers
for i in range(0, len(indexes)):
    markers.append(Marker())
    markers[i].id = i
    markers[i].ns = namespace
    markers[i].action = Marker.ADD

Ejemplo n.º 52
0
    def __init__(self):
        rospy.init_node('dbw_node')

        # Inputs
        vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
        fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
        brake_deadband = rospy.get_param('~brake_deadband', .1)
        decel_limit = rospy.get_param('~decel_limit', -5)
        accel_limit = rospy.get_param('~accel_limit', 1.)
        wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
        wheel_base = rospy.get_param('~wheel_base', 2.8498)
        steer_ratio = rospy.get_param('~steer_ratio', 14.8)
        max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
        max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
        min_speed = 0.1

        self.twist_cmd_sub = rospy.Subscriber(
            "/twist_cmd",
            TwistStamped,
            self.twist_cmd_handler
        )
        self.dbw_enabled_sub = rospy.Subscriber(
            "/vehicle/dbw_enabled",
            Bool,
            self.dbw_enabled_handler
        )
        self.current_velocity_sub = rospy.Subscriber(
            "/current_velocity",
            TwistStamped,
            self.current_velocity_handler
        )

        # State
        self.initialized = False
        self.dbw_enabled = False
        self.current_velocity = None
        self.twist_cmd = None
        self.controller = Controller(
            vehicle_mass=vehicle_mass,
            fuel_capacity=fuel_capacity,
            brake_deadband=brake_deadband,
            decel_limit=decel_limit,
            accel_limit=accel_limit,
            wheel_radius=wheel_radius,
            wheel_base=wheel_base,
            steer_ratio=steer_ratio,
            max_lat_accel=max_lat_accel,
            max_steer_angle=max_steer_angle,
            min_speed=min_speed,
        )

        # Outputs
        self.steering_cmd_pub = rospy.Publisher(
            '/vehicle/steering_cmd',
            SteeringCmd,
            queue_size=1
        )
        self.throttle_cmd_pub = rospy.Publisher(
            '/vehicle/throttle_cmd',
            ThrottleCmd,
            queue_size=1
        )
        self.brake_cmd_pub = rospy.Publisher(
            '/vehicle/brake_cmd',
            BrakeCmd,
            queue_size=1
        )

        self.loop()
Ejemplo n.º 53
0
 def cb_control(self, msg_control):
     self.updateParameters()
     rospy.loginfo(rospy.get_param('/control/manual'))
     rospy.loginfo(rospy.get_param('/control/step'))
     self.control.pid.set_setpoint(self.setpoint)
from std_msgs.msg import Bool
from race.msg import drive_param
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Int32
'''Need rospy and message types for eStop (bool), drive_parameters (drive_param), scan (LaserScan)'''

from math import radians, degrees, pi #for conversions
from math import * #for conversions

from numpy import isnan
import numpy 


#global parameters
'''Drive parameters'''
velocity = rospy.get_param("/initial_speed", "12")
SIDE = rospy.get_param("/initial_side", -1)
#SIDE = 1 is right SIDE = -1 is left

'''Publisher'''
#em_pub = rospy.Publisher('eStop', Bool, queue_size=10)
v_pub  = rospy.Publisher('drive_velocity', Int32, queue_size=1)
activated = False


# 12 --> 1
# 30 --> 2
# 45 --> 5.5
### 5.5 is good enough but 6 for extra safety

FRONT_BUMPER_THRESHOLD = 3.0 
Ejemplo n.º 55
0
    print 'geting parameter from dynamic reconfigure...' 
    isStart = config['isStart']
    isAbort= config['isAbort']
    search_depth=config['search_depth']
    search_distance=config['search_distance']
    z_threshold =config['z_threshold']
    ver_dist_to_pinger=config['altitude_at_pinger']-0.6+0.13 
    #1.13: dist from hydrophone to DVL, 0.5, height of pinger
    return config
if __name__ == '__main__':
    rospy.init_node('acoustic_navigation',log_level=rospy.DEBUG)
    loop_rate=rospy.Rate(0.5)

    rospy.loginfo("AcousticNavigation activated!")
    rospy.wait_for_service('set_controller_srv')
    isTest = rospy.get_param('~testmode',False)
    
    #Setup dynamic reconfigure

    reconfigure_srv=Server(acoustic_navConfig, dynamic_reconfigure_cb)

    #Setup movement client
    movement_client = actionlib.SimpleActionClient('LocomotionServer', bbauv_msgs.msg.ControllerAction)
    movement_client.wait_for_server()

    if isTest:
        isStart= True
        cur_heading=10000 #dummy val
        set_controller_request=rospy.ServiceProxy('set_controller_srv',set_controller)
        set_controller_request(True,True,True,True,False,False,False)
    else:
Ejemplo n.º 56
0
	def init_hardware_test(self):
		
		rospy.init_node('hardware_test')
		
		### INTERNAL PARAMETERS
		self.wait_time = 5		# waiting time (in seconds) before trying initialization again
		self.wait_time_recover = 1
		self.wait_time_diag = 3
		###

		### GET PARAMETERS ###
		# Get actuator parameters
		try:
			params_base = rospy.get_param('/hardware_test/components/base')
			for k in params_bases.keys():
				self.base_params.append(params_base[k])
		except:
			pass

		# Get actuator parameters
		try:
			params_actuator = rospy.get_param('/hardware_test/components/actuators')
			for k in params_actuator.keys():
				self.actuators.append(params_actuator[k])
		except:
			pass
		
		# Get sensor parameters	
		try:
			params = rospy.get_param('/hardware_test/components/sensors')
			for k in params.keys():
				self.sensors.append(params[k])
		except:
			pass
			
		if not self.base_params and not self.actuators and not self.sensors:
			raise NameError('Couldn\'t find any components to test under /component_test namespace. '
							'You need to define at least one component in order to run the program (base, actuator, sensor). '
							'View the documentation to see how to define test-components.')

		# Get log-file directory
		try:
			log_dir = rospy.get_param('/hardware_test/result_dir')
		except:
			log_dir = '/tmp'
			
		# Create and prepare logfile
		self.complete_name = '%s/hardware_test_%s.txt' %(log_dir, time.strftime("%Y%m%d_%H-%M"))
		self.log_file = open(self.complete_name,'w')
		
		
		self.log_file.write('Robotino Hardware test')
		self.log_file.write('\n%s \n%s' %(time.strftime('%d.%m.%Y'), time.strftime('%H:%M:%S')))
		self.print_topic('TESTED COMPONENTS')
		if self.actuators:
			for actuator in self.actuators:
				self.log_file.write('\n  ' + actuator['name'])

		self.print_topic('TEST PARAMETERS')
		params = rospy.get_param('/hardware_test/components')
		for key in params:
			self.log_file.write('\n%s:' %(key))
			params_2 = rospy.get_param('/hardware_test/components/%s' %(key))
			for key_2 in params_2:
				self.log_file.write('\n  %s:' %(key_2))
				params_3 = rospy.get_param('/hardware_test/components/%s/%s' %(key, key_2))
				for key_3, value in params_3.iteritems():
					self.log_file.write('\n    %s: %s' %(key_3, value))
					
		self.print_topic('!!!!!!!!!!!!!!!!!!!!TEST LOG!!!!!!!!!!!!!!!!!!!!')
Ejemplo n.º 57
0
def inverted_palm_regrasp(axis, angle, velocity):
    with open(
            "/home/john/catkin_ws/src/shallow_depth_insertion/config/sdi_config.yaml",
            'r') as stream:
        try:
            config = yaml.safe_load(stream)
        except yaml.YAMLError as exc:
            print(exc)
    pose_target = group.get_current_pose().pose
    pos_initial = [
        pose_target.position.x, pose_target.position.y, pose_target.position.z
    ]
    ori_initial = [
        pose_target.orientation.x, pose_target.orientation.y,
        pose_target.orientation.z, pose_target.orientation.w
    ]
    T_we = tf.TransformListener().fromTranslationRotation(
        pos_initial, ori_initial)
    tcp2fingertip = config['tcp2fingertip']
    contact_A_e = [tcp2fingertip, config['object_thickness'] / 2, 0,
                   1]  #TODO: depends on axis direction
    contact_A_w = np.matmul(T_we, contact_A_e)

    visualization.visualizer(contact_A_w[:3], "s", 0.01, 1)  #DEBUG

    # Interpolate orientation poses via quaternion slerp
    q = helper.axis_angle2quaternion(axis, angle)
    ori_target = tf.transformations.quaternion_multiply(q, ori_initial)
    ori_waypoints = helper.slerp(
        ori_initial, ori_target,
        np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle))

    theta_0 = config['theta_0']
    waypoints = []
    action_name = rospy.get_param('~action_name', 'command_robotiq_action')
    robotiq_client = actionlib.SimpleActionClient(action_name,
                                                  CommandRobotiqGripperAction)

    for psi in range(1, angle + 1):
        # Calculate width
        a = config['delta_0'] * math.cos(math.radians(psi))
        b = config['delta_0'] * math.sin(math.radians(psi))
        c = config['object_thickness'] * math.cos(math.radians(psi))
        d = config['object_thickness'] * math.sin(math.radians(psi))
        opposite = a - d
        width = b + c

        # Calculate position
        if theta_0 + psi <= 90:
            hori = math.fabs(tcp2fingertip * math.cos(
                math.radians(theta_0 + psi))) + math.fabs(
                    (width / 2.0) * math.sin(math.radians(theta_0 + psi)))
            verti = math.fabs(tcp2fingertip * math.sin(
                math.radians(theta_0 + psi))) - math.fabs(
                    (width / 2.0) * math.cos(math.radians(theta_0 + psi)))
        else:
            hori = -math.fabs(tcp2fingertip * math.sin(
                math.radians(theta_0 + psi - 90))) + math.fabs(
                    (width / 2.0) * math.cos(math.radians(theta_0 + psi - 90)))
            verti = math.fabs(tcp2fingertip * math.cos(
                math.radians(theta_0 + psi - 90))) + math.fabs(
                    (width / 2.0) * math.sin(math.radians(theta_0 + psi - 90)))

        if axis[0] > 0:
            pose_target.position.y = contact_A_w[
                1] - hori  # TODO: The only thing I changed for inverted palm regrasp is the (-) sign
            pose_target.position.z = contact_A_w[2] - verti
            #print "CASE 1"
        elif axis[0] < 0:
            pose_target.position.y = contact_A_w[1] + hori
            pose_target.position.z = contact_A_w[2] - verti
            #print "CASE 2"
        elif axis[1] > 0:
            pose_target.position.x = contact_A_w[0] - hori
            pose_target.position.z = contact_A_w[2] + verti
            #print "CASE 3"
        elif axis[1] < 0:
            pose_target.position.x = contact_A_w[0] + hori
            pose_target.position.z = contact_A_w[2] + verti
            #print "CASE 4"

        pose_target.orientation.x = ori_waypoints[psi - 1][0]
        pose_target.orientation.y = ori_waypoints[psi - 1][1]
        pose_target.orientation.z = ori_waypoints[psi - 1][2]
        pose_target.orientation.w = ori_waypoints[psi - 1][3]
        waypoints.append(copy.deepcopy(pose_target))
    (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0)
    retimed_plan = group.retime_trajectory(robot.get_current_state(), plan,
                                           velocity)
    group.execute(retimed_plan, wait=False)

    opening_at_zero = config['max_opening'] - 2 * config['finger_thickness']
    psi = 0
    while psi < angle:
        pose = group.get_current_pose().pose
        q_current = [
            pose.orientation.x, pose.orientation.y, pose.orientation.z,
            pose.orientation.w
        ]
        psi = 2 * math.degrees(math.acos(np.dot(q_current, ori_initial)))
        if psi > 100:
            psi = -(psi - 360)
        a = config['delta_0'] * math.cos(math.radians(psi))
        b = config['delta_0'] * math.sin(math.radians(psi))
        c = config['object_thickness'] * math.cos(math.radians(psi))
        d = config['object_thickness'] * math.sin(math.radians(psi))
        opposite = a - d
        width = b + c
        palm_position = 127 + (config['delta_0'] - a) * 1000
        #pos = int((opening_at_zero - width)/config['opening_per_count'])
        Robotiq.goto(robotiq_client,
                     pos=width + 0.003,
                     speed=config['gripper_speed'],
                     force=config['gripper_force'],
                     block=False)
        dynamixel.set_length(palm_position + 9)
        psi = round(psi, 2)
Ejemplo n.º 58
0
 def updateParameters(self):
     self.setParameters(rospy.get_param('/control/parameters'))
     self.setStepParameters(rospy.get_param('/control/step'))
     self.setManualParameters(rospy.get_param('/control/manual'))
     self.setAutoParameters(rospy.get_param('/control/automatic'))
Ejemplo n.º 59
0
    def __init__(self):
        rospy.init_node('dbw_node', log_level=rospy.INFO)

        vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
        fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
        brake_deadband = rospy.get_param('~brake_deadband', .1)
        decel_limit = rospy.get_param('~decel_limit', -5)
        accel_limit = rospy.get_param('~accel_limit', 1.)
        # Distance from front tires to rear tires
        wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
        # Ratio between turn of steering wheel and turn of wheel
        wheel_base = rospy.get_param('~wheel_base', 2.8498)
        steer_ratio = rospy.get_param('~steer_ratio', 14.8)
        max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
        max_steer_angle = rospy.get_param('~max_steer_angle', 8.)

        self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
                                         SteeringCmd, queue_size=1)
        self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
                                            ThrottleCmd, queue_size=1)
        self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
                                         BrakeCmd, queue_size=1)

        # TODO: Create `Controller` object
        self.controller = Controller(
            vehicle_mass,
            wheel_radius,
            accel_limit,
            decel_limit,
            wheel_base,
            steer_ratio,
            0,
            max_lat_accel,
            max_steer_angle)

        # TODO: Subscribe to all the topics you need to

        # Made up of a Header (with a time stamp, and frame_id), and Twist
        # (linear and angular velocity vectors)
        rospy.Subscriber('/twist_cmd', TwistStamped, self.proposed_cb)
        rospy.Subscriber('/current_velocity', TwistStamped, self.current_cb)
        # Simulator Publishes wheter or not the drive by wire is enabled (only
        # publish if it is)
        rospy.Subscriber('/vehicle/dbw_enabled', Bool, self.dbw_enabled_cb)

        self.proposed_linear_vel = 0.
        self.proposed_angular_vel = 0.
        # self.proposed_time = 0.

        self.current_linear_vel = 0.
        self.current_anuglar_vel = 0.

        # obtain current time from /twist_cmd.header.nsecs (or secs)
        # the time stamp in /current_velocity is empty
        self._time_nsecs = None

        self.dbw_enabled = False

        if WRITE_CSV_LOG:
            # NOTE: this file is created at  /home/<user>/.ros/log/
            self._logfile = open("log.csv", "w")
            fieldnames = ["time_nsecs",
                          "current_linear_vel",
                          "proposed_angular_vel",
                          "proposed_linear_vel",
                          "throttle", "brake", "steering"]

            self._logwriter = csv.DictWriter(self._logfile,
                                             fieldnames=fieldnames)
            self._logwriter.writeheader()

        self.loop()
Ejemplo n.º 60
0
def active_regrasp(axis, angle, velocity, active_distance,
                   psi_active_transition, active_distance_2):
    with open(
            "/home/john/catkin_ws/src/shallow_depth_insertion/config/sdi_config.yaml",
            'r') as stream:
        try:
            config = yaml.safe_load(stream)
        except yaml.YAMLError as exc:
            print(exc)
    pose_target = group.get_current_pose().pose
    pos_initial = [
        pose_target.position.x, pose_target.position.y, pose_target.position.z
    ]
    ori_initial = [
        pose_target.orientation.x, pose_target.orientation.y,
        pose_target.orientation.z, pose_target.orientation.w
    ]
    T_we = tf.TransformListener().fromTranslationRotation(
        pos_initial, ori_initial)
    tcp2fingertip = config['tcp2fingertip']
    error = 0.00
    contact_A_e = [
        tcp2fingertip + random.uniform(-error, error),
        -config['object_thickness'] / 2 + random.uniform(-error, error), 0, 1
    ]  #TODO: depends on axis direction
    contact_A_w = np.matmul(T_we, contact_A_e)

    visualization.visualizer(contact_A_w[:3], "s", 0.01, 1)  #DEBUG

    # Interpolate orientation poses via quaternion slerp
    q = helper.axis_angle2quaternion(axis, angle)
    ori_target = tf.transformations.quaternion_multiply(q, ori_initial)
    ori_waypoints = helper.slerp(
        ori_initial, ori_target,
        np.arange(1.0 / angle, 1.0 + 1.0 / angle, 1.0 / angle))

    theta_0 = config['theta_0']
    waypoints = []
    action_name = rospy.get_param('~action_name', 'command_robotiq_action')
    robotiq_client = actionlib.SimpleActionClient(action_name,
                                                  CommandRobotiqGripperAction)
    for psi in range(1, angle + 1):
        # Calculate width
        a = config['delta_0'] * math.cos(math.radians(psi))
        b = config['delta_0'] * math.sin(math.radians(psi))
        c = config['object_thickness'] * math.cos(math.radians(psi))
        d = config['object_thickness'] * math.sin(math.radians(psi))
        opposite = a - d
        width = b + c

        # Calculate position
        if theta_0 + psi <= 90:
            hori = math.fabs(tcp2fingertip * math.cos(
                math.radians(theta_0 + psi))) + math.fabs(
                    (width / 2.0) * math.sin(math.radians(theta_0 + psi)))
            verti = math.fabs(tcp2fingertip * math.sin(
                math.radians(theta_0 + psi))) - math.fabs(
                    (width / 2.0) * math.cos(math.radians(theta_0 + psi)))
        else:
            hori = -math.fabs(tcp2fingertip * math.sin(
                math.radians(theta_0 + psi - 90))) + math.fabs(
                    (width / 2.0) * math.cos(math.radians(theta_0 + psi - 90)))
            verti = math.fabs(tcp2fingertip * math.cos(
                math.radians(theta_0 + psi - 90))) + math.fabs(
                    (width / 2.0) * math.sin(math.radians(theta_0 + psi - 90)))

        if axis[0] > 0:
            pose_target.position.y = contact_A_w[1] + hori
            pose_target.position.z = contact_A_w[2] + verti
            print "CASE 1"
        elif axis[0] < 0:
            if psi <= psi_active_transition:
                pose_target.position.y = contact_A_w[
                    1] - hori - active_distance * psi / psi_active_transition
            else:
                pose_target.position.y = contact_A_w[
                    1] - hori - active_distance + active_distance_2 * (
                        psi - psi_active_transition) / (angle + 1 -
                                                        psi_active_transition)
            pose_target.position.z = contact_A_w[2] + verti
            print "CASE 2"
        elif axis[1] > 0:
            pose_target.position.x = contact_A_w[0] - hori
            pose_target.position.z = contact_A_w[2] + verti
            print "CASE 3"
        elif axis[1] < 0:
            pose_target.position.x = contact_A_w[0] + hori
            pose_target.position.z = contact_A_w[2] + verti
            print "CASE 4"

        pose_target.orientation.x = ori_waypoints[psi - 1][0]
        pose_target.orientation.y = ori_waypoints[psi - 1][1]
        pose_target.orientation.z = ori_waypoints[psi - 1][2]
        pose_target.orientation.w = ori_waypoints[psi - 1][3]
        waypoints.append(copy.deepcopy(pose_target))
    (plan, fraction) = group.compute_cartesian_path(waypoints, 0.01, 0)
    retimed_plan = group.retime_trajectory(robot.get_current_state(), plan,
                                           velocity)
    group.execute(retimed_plan, wait=False)

    opening_at_zero = config['max_opening'] - 2 * config['finger_thickness']
    psi = 0
    while psi < angle:
        pose = group.get_current_pose().pose
        q_current = [
            pose.orientation.x, pose.orientation.y, pose.orientation.z,
            pose.orientation.w
        ]
        psi = 2 * math.degrees(math.acos(np.dot(q_current, ori_initial)))
        if psi > 100:
            psi = -(psi - 360)
        a = config['delta_0'] * math.cos(math.radians(psi))
        b = config['delta_0'] * math.sin(math.radians(psi))
        c = config['object_thickness'] * math.cos(math.radians(psi))
        d = config['object_thickness'] * math.sin(math.radians(psi))
        opposite = a - d
        width = b + c
        #pos = int((opening_at_zero - width)/config['opening_per_count'])
        Robotiq.goto(
            robotiq_client,
            pos=width + 0.000,
            speed=config['gripper_speed'],
            force=config['gripper_force'],
            block=False)  #0.006 for coin; 0.000 for book; 0.005 for poker
        #if psi < 1.0 or psi > 47.0:
        #print "psi= ", psi, "          width= ", width
        psi = round(psi, 2)
        rospy.sleep(0.5)  # TESTING TO SEE IF THE GRIPPER ACTION DOESNT LAG
    return width