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()
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.")
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")
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
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")
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)
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)
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']}
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)
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)
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");
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()
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
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()
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)
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'
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)
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()
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)
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()
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()
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")
def _post_init(self): if not rospy.get_param('~lazy', True): self.subscribe() self._connection_status = True
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")
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
#!/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")
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)
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
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)
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)
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()
def setupParameter(self, param_name, default_value): value = rospy.get_param(param_name, default_value) rospy.set_param(param_name, value) #Write to parameter server for transparancy rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value)) return value
def __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)
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')
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 ===
def loadParameters(self): self.droneList = rospy.get_param('~droneList', ['cf2', 'cf3']) self.NumDrones = len(self.droneList) pass
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
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()
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
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:
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!!!!!!!!!!!!!!!!!!!!')
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)
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'))
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()
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