def setUp(self): rospy.init_node('test_motion_execution_buffer') self.tf = TransformListener() self.obj_pub = rospy.Publisher('collision_object', CollisionObject, latch=True) self.move_arm_action_client = actionlib.SimpleActionClient( "move_right_arm", MoveArmAction) self.move_arm_action_client.wait_for_server() obj1 = CollisionObject() obj1.header.stamp = rospy.Time.now() obj1.header.frame_id = "base_link" obj1.id = "obj1" obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD obj1.shapes = [Shape() for _ in range(1)] obj1.shapes[0].type = Shape.CYLINDER obj1.shapes[0].dimensions = [float() for _ in range(2)] obj1.shapes[0].dimensions[0] = .1 obj1.shapes[0].dimensions[1] = 1.5 obj1.poses = [Pose() for _ in range(1)] obj1.poses[0].position.x = .6 obj1.poses[0].position.y = -.6 obj1.poses[0].position.z = .75 obj1.poses[0].orientation.x = 0 obj1.poses[0].orientation.y = 0 obj1.poses[0].orientation.z = 0 obj1.poses[0].orientation.w = 1 self.obj_pub.publish(obj1) rospy.sleep(2.0)
def __init__(self): if not rospy.core.is_initialized(): rospy.init_node('laser_test') rospy.loginfo("Initialised rospy node: laser_test") self.tl = TransformListener() self.lp = LaserProjection() # Publishers self.all_laser_pub = rospy.Publisher('/pepper/laser_2', LaserScan, queue_size=1) self.pc_pub = rospy.Publisher('/cloud', PointCloud2, queue_size=1) self.pcl_pub = rospy.Publisher('/cloudl', PointCloud2, queue_size=1) self.pcr_pub = rospy.Publisher('/cloudr', PointCloud2, queue_size=1) self.pc_redone_pub = rospy.Publisher('/cloud_redone', PointCloud2, queue_size=1) self.pc_rereprojected_pub = rospy.Publisher('/cloud_rereprojected', PointCloud2, queue_size=1) # Subscribers left_sub = Subscriber('/pepper/scan_left', LaserScan) front_sub = Subscriber('/pepper/scan_front', LaserScan) right_sub = Subscriber('/pepper/scan_right', LaserScan) self.ts = TimeSynchronizer([left_sub, front_sub, right_sub], 10) rospy.loginfo("Finished intialising") self.ddr = DDR('increment') default_increment = radians(120.0 * 2.0) / 61.0 self.ddr.add_variable('angle_increment', '', default_increment, 0.05, 0.08) # 130.665 self.ddr.add_variable('half_max_angle', '', 120., 115., 145.0) self.ddr.start(self.dyn_rec_callback) self.ts.registerCallback(self.scan_cb) rospy.loginfo("Ready to go.")
def __init__(self): self._camera_frame = rospy.get_param('%s/camera_frame' % (NODE_NAME, ), 'camera_rgb_optical_frame') self._marker_frame = rospy.get_param('%s/marker_frame' % (NODE_NAME, ), 'marker26') self._robot_base_frame = rospy.get_param( '%s/robot_base_frame' % (NODE_NAME, ), 'world') self._ee_frame = rospy.get_param('%s/ee_frame' % (NODE_NAME, ), 'left_arm_7_link') self._save_calib_file = rospy.get_param( '%s/save_calib_file' % (NODE_NAME, ), 'hand_eye_calib_data.npy') self._load_calib_file = rospy.get_param( '%s/load_calib_file' % (NODE_NAME, ), 'hand_eye_calib_data.npy') self._calib_from_file = rospy.get_param( '%s/calibrate_from_file' % (NODE_NAME, ), False) rospy.loginfo("Camera frame: %s" % (self._camera_frame, )) rospy.loginfo("Marker frame: %s" % (self._marker_frame, )) rospy.loginfo("Robot frame: %s" % (self._robot_base_frame, )) rospy.loginfo("End-effector frame: %s" % (self._ee_frame, )) rospy.loginfo("Calibration filename to save: %s" % (self._save_calib_file, )) rospy.loginfo("Calibration filename to load: %s" % (self._load_calib_file, )) rospy.loginfo("Calibrate from file: %s" % (self._calib_from_file, )) self._camera_base_p = [0.000, 0.022, 0.0] self._camera_base_q = [-0.500, 0.500, -0.500, 0.500] self._camera_base_transform = self.to_transform_matrix( self._camera_base_p, self._camera_base_q) self._calib = HandEyeCalib() self._br = tf.TransformBroadcaster() self._tf = TransformListener()
def __init__(self): cv2.namedWindow("map") cv2.namedWindow("past_map") rospy.init_node("run_mapping") #create map properties, helps to make ratio calcs self.origin = [-10, -10] self.seq = 0 self.resolution = 0.1 self.n = 200 self.pose = [] self.dyn_obs = [] self.rapid_appear = set() self.counter = 0 #Giving initial hypotheses to the system self.p_occ = 0.5 * np.ones( (self.n, self.n)) #50-50 chance of being occupied self.odds_ratio_hit = 3.0 #this is arbitrary, can re-assign self.odds_ratio_miss = 0.3 #this is arbitrary, can reassign #TODO: Evaluate these - what do we need to change in order to make this more friendly to our version? Potential changes: #Whenever there is an adjustment to self.odds_ratio_miss, update an odds ratio that implies dynamicness #calculates odds based upon hit to miss, equal odds to all grid squares self.odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones( (self.n, self.n)) #calculate initial past odds_ratio self.past_odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones( (self.n, self.n)) #TODO: Make sure that this is still an accurate representation of how probabilities work in our system. Make appropriate additions/adjustments for the dynamic obstacle piece #write laser pubs and subs rospy.Subscriber("scan", LaserScan, self.scan_received, queue_size=1) self.pub = rospy.Publisher("map", OccupancyGrid) #note - in case robot autonomy is added back in self.tf_listener = TransformListener()
def __init__(self, server_prefix, group_name): self.group_name = group_name self.group = moveit_commander.MoveGroupCommander(self.group_name) self._as = actionlib.SimpleActionServer( server_prefix + self.group_name + "/manipulation", ArmNavigationAction, self.action_cb) self._as.start() char = self.group_name[0] self.jta = actionlib.SimpleActionClient( '/' + char + '_arm_controller/joint_trajectory_action', JointTrajectoryAction) rospy.loginfo('Waiting for joint trajectory action') self.jta.wait_for_server() rospy.loginfo('Got it') self.joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.js_cb, queue_size=1) self.angles = None self.move_to_pose_pub = rospy.Publisher( "/pr2_arm_navigation/move_to_pose", PoseStamped, queue_size=10, latch=True) self.joint_names = [ char + '_shoulder_pan_joint', char + '_shoulder_lift_joint', char + '_upper_arm_roll_joint', char + '_elbow_flex_joint', char + '_forearm_roll_joint', char + '_wrist_flex_joint', char + '_wrist_roll_joint' ] self.tf_listener = TransformListener() self.look_at_pub = rospy.Publisher("/art/robot/look_at", PointStamped, queue_size=1)
def __init__(self): self.namespace = 'tello/' self.hover_position = None # subscribe to target pose self.target_position = PoseStamped() self.target_angles = [0.0, 0.0, 0.0] # Initialize the tf listener self.listener = TransformListener() # initialize publisher for tello command velocity (geometry_Twist) self.fly_msg = self.fly_tello_msg = Twist() # set the fly command self.velocity_pub = rospy.Publisher(self.namespace + 'cmd_vel', Twist, queue_size=1) self.takeoff_pub = rospy.Publisher(self.namespace + 'takeoff', Empty, queue_size=1) self.land_pub = rospy.Publisher(self.namespace + 'land', Empty, queue_size=1)
def __init__(self, use_dummy_transform=False): rospy.init_node('star_center_positioning_node') if use_dummy_transform: self.odom_frame_name = "odom_dummy" else: self.odom_frame_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.pose_correction = rospy.get_param('~pose_correction',0.0) self.marker_sub = rospy.Subscriber("ar_pose_marker", ARMarkers, self.process_markers) self.odom_sub = rospy.Subscriber("odom", Odometry, self.process_odom, queue_size=10) self.star_pose_pub = rospy.Publisher("STAR_pose",PoseStamped,queue_size=10) self.continuous_pose = rospy.Publisher("STAR_pose_continuous",PoseStamped,queue_size=10) self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster()
def __init__(self): if World.tf_listener == None: World.tf_listener = TransformListener() self._lock = threading.Lock() self.surface = None self._tf_broadcaster = TransformBroadcaster() self._im_server = InteractiveMarkerServer('world_objects') bb_service_name = 'find_cluster_bounding_box' rospy.wait_for_service(bb_service_name) self._bb_service = rospy.ServiceProxy(bb_service_name, FindClusterBoundingBox) rospy.Subscriber('interactive_object_recognition_result', GraspableObjectList, self.receieve_object_info) self._object_action_client = actionlib.SimpleActionClient( 'object_detection_user_command', UserCommandAction) self._object_action_client.wait_for_server() rospy.loginfo('Interactive object detection action ' + 'server has responded.') self.clear_all_objects() # The following is to get the table information rospy.Subscriber('tabletop_segmentation_markers', Marker, self.receieve_table_marker)
def __init__(self, context): super(GaitGeneratorPlugin, self).__init__(context) # Default values self.gait_publisher = None self.topic_name = '' self.gait_directory = None self.playback_speed = 100 self.time_slider_thread = None self.current_time = 0 self.tf_listener = TransformListener() self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10) self.joint_changed_history = RingBuffer(capacity=100, dtype=list) self.joint_changed_redo_list = RingBuffer(capacity=100, dtype=list) self.robot = urdf.Robot.from_parameter_server() self.gait = ModifiableSubgait.empty_subgait(self, self.robot) self.build_ui(context) self.set_topic_name(self.topic_name_line_edit.text()) self.load_gait_into_ui()
def __init__(self): rospy.init_node('thruster_controls') self.sim = rospy.get_param('~/thruster_controls/sim') if not self.sim: self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC, ThrusterSpeeds, queue_size=3) else: self.pub = rospy.Publisher(self.SIM_PUB_TOPIC, Float32MultiArray, queue_size=3) self.enable_service = rospy.Service('enable_controls', SetBool, self.handle_enable_controls) self.tm = ThrusterManager(rr.get_filename('package://controls/config/cthulhu.config', use_protocol=False)) self.listener = TransformListener() for d in utils.get_axes(): rospy.Subscriber(utils.get_controls_move_topic(d), Float64, self._on_pid_received, d) rospy.Subscriber(utils.get_power_topic(d), Float64, self._on_power_received, d) self.pid_outputs = np.zeros(6) self.pid_outputs_local = np.zeros(6) self.powers = np.zeros(6) self.t_allocs = np.zeros(8)
def test_tf(self): rospy.init_node('morse_ros_tf_test') self.tf = TransformListener() sleep(1) self.assertTrue(self.tf.frameExists("/odom")) self.assertTrue(self.tf.frameExists("/base_footprint")) self.assertTrue(self.tf.frameExists("/map")) self.assertTrue(self.tf.frameExists("/robot2")) self._check_pose("odom", "base_footprint", [0, 0, 0], [0, 0, 0, 1]) self._check_pose("map", "robot2", [0, 0, 0], [0, 0, 0, 1]) with Morse() as morse: teleport = morse.robot.motion teleport.publish({'x' : 2, 'y' : 0, 'z' : 0, \ 'yaw' : 0, 'pitch' : 0.0, 'roll' : 0.0}) morse.sleep(0.1) self._check_pose("odom", "base_footprint", [2, 0, -0.1], [0, 0, 0, 1])
def ros_setup(self): """Creates and inits ROS components""" RComponent.ros_setup(self) self.tf = TransformListener() self.move_action_client = actionlib.SimpleActionClient( self.move_action_name, MoveAction) self.dock_action_client = actionlib.SimpleActionClient( self.dock_action_name, DockAction) self.start_docking_server = rospy.Service( '~start', Trigger, self.start_docking_service_cb) self.stop_docking_server = rospy.Service('~stop', Trigger, self.stop_docking_service_cb) self.save_results_server = rospy.Service('~save_results', Empty, self.save_results_service_cb) self.docking_status = rospy.Publisher('~status', String, queue_size=10) return 0
def __init__(self): self.node_name = rospy.get_name() self.tf_listener = TransformListener() # Re-publishing Robot's states in the format needed for the network self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback) self.pose_pub = rospy.Publisher('/robot_pose', PoseStamped, queue_size=10) self.vel_pub = rospy.Publisher('/robot_velocity', Vector3, queue_size=10) # Re-publishing Pedestrians states in the format needed for the network self.ped_sub = rospy.Subscriber( '/spencer/perception_internal/detected_person_association/lasers_upper_body_fused', DetectedPersons, self.peds_callback) self.clusters_pub = rospy.Publisher('cluster/output/clusters', Clusters, queue_size=1) #self.pub_agent_markers = rospy.Publisher('~agent_markers',MarkerArray,queue_size=1) #publish goal self.goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1)
def __init__(self, arm_parameters): """ Initializes the Palbator Moveit arm controller. """ moveit_commander.roscpp_initialize(sys.argv) # rospy.init_node('move_group_python_interface_tutorial', anonymous=True) self._parameters = arm_parameters self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander( self._parameters['Palbator_arm_move_group']) self.display_trajectory_publisher = rospy.Publisher( self._parameters['display_arm_planned_path_topic'], moveit_msgs.msg.DisplayTrajectory, queue_size=10) self._tflistener = TransformListener() self.arm_length = self._parameters['Palbator_arm_length'] rospy.logwarn("{class_name} : ARM CONTROLLER ON".format( class_name=self.__class__.__name__))
def __init__(self): # Initialize arm state. self._tf_listener = TransformListener() self._arm = Arm(self._tf_listener) self._arm.close_gripper() self._status = ExecutionStatus.NOT_EXECUTING rospy.Service('/fetch_pbd/move_arm_to_joints_plan', MoveArm, self._move_to_joints_plan) rospy.Service('/fetch_pbd/move_arm_to_joints', MoveArmTraj, self._move_to_joints) rospy.Service('/fetch_pbd/move_arm_to_pose', MoveArm, self._move_to_pose) rospy.Service('/fetch_pbd/start_move_arm_to_pose', MoveArm, self._start_move_to_pose) rospy.Service('/fetch_pbd/is_reachable', MoveArm, self._is_reachable) rospy.Service('/fetch_pbd/is_arm_moving', GetArmMovement, self._is_arm_moving) rospy.Service('/fetch_pbd/relax_arm', Empty, self._relax_arm) rospy.Service('/fetch_pbd/reset_arm_movement_history', Empty, self._reset_movement_history) rospy.Service('/fetch_pbd/get_gripper_state', GetGripperState, self._get_gripper_state) rospy.Service('/fetch_pbd/get_ee_pose', GetEEPose, self._get_ee_pose) rospy.Service('/fetch_pbd/get_joint_states', GetJointStates, self._get_joint_states) rospy.Service('/fetch_pbd/set_gripper_state', SetGripperState, self._set_gripper_state) rospy.loginfo('Arm initialized.')
def __init__(self): print('Initializing') self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('localizer') self.pf = ParticleFilter() self.base_frame = "base_link" # Robot base frame self.map_frame = "map" # Map coord frame self.odom_frame = "odom" # Odom coord frame self.scan_topic = "scan" # Laser scan topic self.linear_threshold = 0.1 # the amount of linear movement before performing an update self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model self.odom_pose = PoseStamped() self.robot_pose = Pose() self.robot_pose = Pose() self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan) # init pf # subscribers and publisher self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_particle_updater) rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.pose_updater) # enable listening for and broadcasting coord transforms self.tf_listener = TransformListener() self.tf_broadcaster = TransformBroadcaster() self.occupancy_field = OccupancyField() self.transform_helper = TFHelper() self.current_odom_xy_theta = [] print("initialization complete") self.initialized = True
def __init__(self, name, odom_frame, base_frame): """ @param name: the name of the action @param odom_frame: the frame the robot is moving in (odom_combined) @param base_frame: the vehicles own frame (usually base_link) """ self._action_name = name self.__odom_frame = odom_frame self.__base_frame = base_frame self.__server = actionlib.SimpleActionServer(self._action_name, sprayAction, auto_start=False) self.__server.register_preempt_callback(self.preempt_cb) self.__server.register_goal_callback(self.goal_cb) self.__cur_pos = 0 self.__start_pos = 0 self.__start_yaw = 0 self.__cur_yaw = 0 self.__feedback = sprayFeedback() self.__listen = TransformListener() #self.vel_pub = rospy.Publisher("/fmControllers/cmd_vel_auto",Twist) self.spray_pub = rospy.Publisher("/fmControllers/spray", UInt32) self.__start_time = rospy.Time.now() self.new_goal = False self.n_sprays = 0 self.spray_msg = UInt32() self.spray_l = False self.spray_cnt = 0 self.__server.start()
def __init__(self, moveit_group, initial_joint_pos=INITIAL_JOINT_STATE, control_mode='position', extended_finger=True): """ Sawyer class. :param initial_joint_pos: {str: float} {'joint_name': position_value}, and also initial_joint_pos should include all of the joints that user wants to control and observe. :param moveit_group: str Use this to check safety :param control_mode: string robot control mode: 'position' or velocity or effort """ Robot.__init__(self) self._limb = intera_interface.Limb('right') self._gripper = intera_interface.Gripper() self._initial_joint_pos = initial_joint_pos self._control_mode = control_mode self._used_joints = [] for joint in initial_joint_pos: self._used_joints.append(joint) self._joint_limits = rospy.wait_for_message('/robot/joint_limits', JointLimits) self._moveit_group = moveit_group self._tf_listener = TransformListener() self._base_frame = "base" if extended_finger: self._tip_frame = "right_gripper_tip_ex" else: self._tip_frame = "right_gripper_tip" self._sv = StateValidity()
def __init__(self): rospy.loginfo("Waiting for the tf to come up") rospy.sleep(2) rospy.init_node('sweep_nav') sub = rospy.Subscriber('/imu/mag', MagneticField, self.callback, queue_size=10) self.pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10) self.pub2 = rospy.Publisher('way_point', Marker, queue_size=10) self.pub3 = rospy.Publisher('z_mag', Marker, queue_size=10) self.marker = Marker() self.id_new = 0 self.id_old = 0 self.count = 1 self.z = [] self.marker.header.frame_id = 'map' self.marker.type = 3 self.marker.action = self.marker.ADD self.marker.scale.x = 0.1 self.marker.scale.y = 0.1 self.marker.color.a = 0.5 self.marker.scale.z = 0.1 self.tf = TransformListener() self.vel = Twist() self.speed = 0.15 self.X_KOUSHI = 150 self.Y_KOUSHI = 150 self.id_list = [[i + self.X_KOUSHI * j for i in range(self.X_KOUSHI)] for j in range(self.Y_KOUSHI)] self.mag = 0 self.f = open('../map/way.txt', 'a') self.now = rospy.get_time()
def __init__(self, configs): """ Constructor of the SimpleCamera class. :param configs: Camera specific configuration object :type configs: YACS CfgNode """ super(SimpleCamera, self).__init__(configs=configs) self.cv_bridge = CvBridge() self.camera_info_lock = threading.RLock() self.camera_img_lock = threading.RLock() self._tf_listener = TransformListener() self.rgb_img = None self.depth_img = None self.camera_info = None self.camera_P = None rospy.Subscriber(self.configs.CAMERA.ROSTOPIC_CAMERA_INFO_STREAM, CameraInfo, self._camera_info_callback) rgb_topic = self.configs.CAMERA.ROSTOPIC_CAMERA_RGB_STREAM self.rgb_sub = message_filters.Subscriber(rgb_topic, Image) depth_topic = self.configs.CAMERA.ROSTOPIC_CAMERA_DEPTH_STREAM self.depth_sub = message_filters.Subscriber(depth_topic, Image) img_subs = [self.rgb_sub, self.depth_sub] self.sync = message_filters.ApproximateTimeSynchronizer(img_subs, queue_size=10, slop=0.2) self.sync.registerCallback(self._sync_callback) depth_threshold = (self.configs.BASE.VSLAM.DEPTH_MIN, self.configs.BASE.VSLAM.DEPTH_MAX) cfg_filename = self.configs.BASE.VSLAM.CFG_FILENAME self.depth_cam = DepthImgProcessor(subsample_pixs=1, depth_threshold=depth_threshold, cfg_filename=cfg_filename) self.cam_cf = self.configs.BASE.VSLAM.RGB_CAMERA_CENTER_FRAME self.base_f = self.configs.BASE.VSLAM.VSLAM_BASE_FRAME
def __init__(self): self.choice = 0 self.movePub = rospy.Publisher( '/cmd_vel', Twist, queue_size=10) # publisher for move commands self.peopleSub = rospy.Subscriber( '/people_tracker_filter/people', People, self.peopleCall) # subscribe to people tracker self.laserSub = rospy.Subscriber('/scan', LaserScan, self.laserCall) # subscribe to laser self.distance = 0 # variable to store distance from laser self.tf = TransformListener() while True: print("select behaviour") print("1) control") print("2) friendly") print("3) cautious") self.choice = input("> ") if self.choice == 1 or self.choice == 2: break elif self.choice == 3: break print("started")
def read_from_pickle(): global points_description, tf_listner rospy.init_node('point_test') tf_listner = TransformListener() points_description = pickle.load( open( rospkg.RosPack().get_path('robot_describe') + "/script/data/points_describtion.p", "rb")) position_points = [[i[0].x, i[0].y] for i in points_description] description_degree = [[i[1], i[2]] for i in points_description] # for points in description_degree: # print points points_description = [position_points, description_degree] print description_degree robot = Robot(True, 2, start_index_dataset=1140, use_direction_file=False, bag_name="/bag_train/") rospy.Subscriber("/robot_0/base_pose_ground_truth", Odometry, callback_robot_0, robot) rospy.Subscriber("/robot_0/base_scan_1", LaserScan, callback_laser_scan, robot) rospy.spin()
def __init__(self): self.initialized = False # make sure we don't perform updates before everything is setup rospy.init_node('RMI_pf') self.base_frame = "base_link" # the frame of the robot base self.map_frame = "map" # the name of the map coordinate frame self.odom_frame = "odom" # the name of the odometry coordinate frame self.scan_topic = "scan" # the topic where we will get laser scans from self.n_particles = 20 self.linear_mov = 0.1 self.angular_mov = math.pi / 10 self.laser_max_distance = 2.0 self.sigma = 0.05 # Descomentar essa linha caso /initialpose seja publicada # self.pose_listener = rospy.Subscriber("initialpose", # PoseWithCovarianceStamped, # self.update_initial_pose) self.laser_subscriber = rospy.Subscriber(self.scan_topic, LaserScan, self.scan_received) self.particle_pub = rospy.Publisher("particlecloud_rmi", PoseArray, queue_size=1) self.tf_listener = TransformListener() self.particle_cloud = [] self.current_odom_xy_theta = [] self.map_server = rospy.ServiceProxy('static_map', GetMap) self.map = self.map_server().map self.occupancy_field = OccupancyField(self.map) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(1.0)) self.initialized = True
def __init__(self): self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() rospy.Subscriber("joy", Joy, self._joyChanged) rospy.Subscriber("cmd_vel_telop", Twist, self._cmdVelTelopChanged) self.cmd_vel_telop = Twist() #self.pidX = PID(20, 12, 0.0, -30, 30, "x") #self.pidY = PID(-20, -12, 0.0, -30, 30, "y") #self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 60000, "z") #self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.pidX = PID(20, 12.0, 0.2, -30, 30, "x") self.pidY = PID(-20, -12.0, -0.2, -30, 30, "y") #50000 800 self.pidZ = PID(15000, 3000.0, 1500.0, 10000, 57000, "z") self.pidYaw = PID(50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.state = Controller.Manual self.targetZ = 1 self.targetX = 0.0 self.targetY = -1.0 self.des_angle = 90.0 self.lastZ = 0.0 self.power = 50000.0 self.pubVz = rospy.Publisher('vel_z', Float32, queue_size=1) self.lastJoy = Joy()
def __init__(self): rospy.init_node('thruster_controls') self.sim = rospy.get_param('~/thruster_controls/sim') if self.sim == False: self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC, ThrusterSpeeds, queue_size=3) elif self.sim == True: self.pub = rospy.Publisher(self.SIM_PUB_TOPIC, Float32MultiArray, queue_size=3) else: # TODO: alert that unrecognized mode destination has been set pass self.enable_service = rospy.Service('enable_controls', SetBool, self.handle_enable_controls) self.tm = ThrusterManager(os.path.join(sys.path[0], '../config/cthulhu.config')) self.listener = TransformListener() rospy.Subscriber(self.CONTROLS_MOVE_X_TOPIC, Float64, self._on_x) rospy.Subscriber(self.CONTROLS_MOVE_Y_TOPIC, Float64, self._on_y) rospy.Subscriber(self.CONTROLS_MOVE_Z_TOPIC, Float64, self._on_z) rospy.Subscriber(self.CONTROLS_MOVE_ROLL_TOPIC, Float64, self._on_roll) rospy.Subscriber(self.CONTROLS_MOVE_PITCH_TOPIC, Float64, self._on_pitch) rospy.Subscriber(self.CONTROLS_MOVE_YAW_TOPIC, Float64, self._on_yaw) rospy.Subscriber(self.CONTROLS_POWER_X_TOPIC, Float64, self._on_x_power) rospy.Subscriber(self.CONTROLS_POWER_Y_TOPIC, Float64, self._on_y_power) rospy.Subscriber(self.CONTROLS_POWER_Z_TOPIC, Float64, self._on_z_power) rospy.Subscriber(self.CONTROLS_POWER_ROLL_TOPIC, Float64, self._on_roll_power) rospy.Subscriber(self.CONTROLS_POWER_PITCH_TOPIC, Float64, self._on_pitch_power) rospy.Subscriber(self.CONTROLS_POWER_YAW_TOPIC, Float64, self._on_yaw_power) self.pid_outputs = np.zeros(6) self.pid_outputs_local = np.zeros(6) self.powers = np.zeros(6) self.t_allocs = np.zeros(8)
def __init__(self): cv2.namedWindow("map") cv2.namedWindow("past_map") rospy.init_node("run_mapping") #create map properties, helps to make ratio calcs self.origin = [-10, -10] self.seq = 0 self.resolution = 0.1 self.n = 200 self.pose = [] self.cluster_pose = [] self.dyn_obs = [] self.rapid_appear = set() self.counter = 0 #Giving initial hypotheses to the system self.p_occ = 0.5 * np.ones( (self.n, self.n)) #50-50 chance of being occupied self.odds_ratio_hit = 3.0 #this is arbitrary, can re-assign self.odds_ratio_miss = 0.3 #this is arbitrary, can reassign #calculates odds based upon hit to miss, equal odds to all grid squares self.odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones( (self.n, self.n)) #calculate initial past odds_ratio self.past_odds_ratios = (self.p_occ) / (1 - self.p_occ) * np.ones( (self.n, self.n)) #write laser pubs and subs rospy.Subscriber("scan", LaserScan, self.scan_received, queue_size=1) self.pub = rospy.Publisher("map", OccupancyGrid) #note - in case robot autonomy is added back in self.tf_listener = TransformListener()
def __init__(self, topic='/hace/people', x_min=0, x_max=0.5, y_min=-0.75, y_max=0.05, z_min=-0.25, z_max=0.55): super(HandoverAdaptionInit, self).__init__(outcomes=['right_hand_in_ws', 'left_hand_in_ws', 'error']) rospy.loginfo('Starting initiation state.') # define workspace self._x_min = x_min self._x_max = x_max self._y_min = y_min self._y_max = y_max self._z_min = z_min self._z_max = z_max self._ws_transformed = False self._tf = TransformListener() self._topic = topic self._person = None self._persons = None self._sub = ProxySubscriberCached({self._topic: MinimalHumans}) self._error = False
def __init__(self): self.robot = hsrb_interface.Robot() self.noise = 0.1 self.omni_base = self.robot.get('omni_base') self.whole_body = self.robot.get('whole_body') self.gripper = self.robot.get('gripper') self.tl = TransformListener() self.start_recording = False self.stop_recording = False self.com = COM() self.com.go_to_initial_state(self.whole_body, self.gripper) #self.whole_body.move_to_joint_positions({'head_tilt_joint':-0.3}) self.cam = RGBD() self.joystick = JoyStick() self.torque = Gripper_Torque() self.joints = Joint_Positions()
def __init__(self): # Get information for this particular camera camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo) print camera_info # Set information for the camera self.cam_model = PinholeCameraModel() self.cam_model.fromCameraInfo(camera_info) # Subscribe to the camera's image topic and depth image topic self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw", Image, self.on_rgb) self.depth_image_sub = rospy.Subscriber( "head_camera/depth_registered/image_raw", Image, self.on_depth) # Publish where the button is in the base link frame self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10) # Set up connection to CvBridge self.bridge = CvBridge() # Open up viewing windows cv2.namedWindow('depth') cv2.namedWindow('rgb') # Set up the class variables self.rgb_image = None self.rgb_image_time = None self.depth_image = None self.center = None self.return_point = PointStamped() self.tf_listener = TransformListener()
def pid_handle(): """ Temporary function to cope with the keyboard input error of click """ global car_rotation, car_translation scan_listener = TransformListener() rospy.init_node("pid_local", anonymous=True) MASTER_MAP.intial_state_listener("/map", "/odom") way_point_coord = [] for i in WAY_POINT_GRID: way_point_coord.append(MASTER_MAP.grid_to_coord(i)) # get initial time to setup the pid controller init_time = rospy.Time.now().secs + rospy.Time.now().nsecs * (10**-9) # set pit controller pid_driver = PIDController(MAX_SPEED, MAX_ANGLE, PID_CONST, init_time, way_point_coord) point_export = rospy.Publisher(PATH_TOPIC, Float32MultiArray, queue_size=2) rospy.Subscriber( ODOM_TOPIC, Odometry, callback_pid, callback_args=[pid_driver, pid_pub, point_export], ) rospy.Subscriber(ODOM_TOPIC, Odometry, odom_tf_callback, queue_size=10) rospy.Subscriber(LASER_TOPIC, LaserScan, scan_callback, queue_size=10)