def start(self): self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_trafo)
array_data = [now.secs,now.nsecs,\ x,y,psi,x_vel,y_vel,psi_vel,\ x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\ x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl] data_to_send = Float64MultiArray(data = array_data) pub_data.publish(data_to_send) elif(joy_button_L1 == 1 or joy_button_R1 == 1): twist.linear.x = joy_x twist.linear.y = joy_y twist.angular.z = joy_z pub_velocity.publish(twist) else: # ----- idle if no goals ----- pub_velocity.publish(Twist()) if __name__ == '__main__': rospy.init_node('controller', anonymous=True) # PUBLISHER pub_velocity = rospy.Publisher('/mallard/thruster_command',Twist,queue_size=10) pub_data = rospy.Publisher('/mallard/controller_data',Float64MultiArray,queue_size=10) # SUBSCRIBER rospy.Subscriber("/joy",Joy,joy_callback) # rospy.Subscriber("/slam_out_pose",PoseStamped,slam_callback) rospy.Subscriber("/vicon_pose",PoseStamped,slam_callback) rospy.Subscriber("/mallard/goals",Float64MultiArray,goal_callback) rospy.Timer(rospy.Duration(0.1), control_callback,oneshot=False) rospy.spin()
def __init__(self): #init stuff #get stuff self.num_landmarks = 12 # Estimator stuff # x = pn, pe, pd, phi, theta, psi self.xhat = np.zeros((9 + 3 * self.num_landmarks, 1)) self.xhat_odom = Odometry() # Covariance matrix self.P = np.zeros( (9 + 3 * self.num_landmarks, 9 + 3 * self.num_landmarks)) self.P[9:, 9:] = np.eye(3 * self.num_landmarks) * 9999999.9 # Inf self.Q = np.diag([10.0, 5.0, 5.0]) # meas noise # Measurements stuff # Truth self.truth_pn = 0.0 self.truth_pe = 0.0 self.truth_pd = 0.0 self.truth_phi = 0.0 self.truth_theta = 0.0 self.truth_psi = 0.0 self.truth_p = 0.0 self.truth_q = 0.0 self.truth_r = 0.0 self.truth_pndot = 0.0 self.truth_pedot = 0.0 self.truth_pddot = 0.0 self.truth_u = 0.0 self.truth_v = 0.0 self.truth_w = 0.0 self.prev_time = 0.0 self.imu_az = 0.0 #aruco Stuff self.aruco_location = { 100: [0.0, 0.0, 0.0], 101: [0.0, -14.5, 5.0], 102: [5.0, -14.5, 5.0], 103: [-5.0, -14.5, 5.0], 104: [0.0, 14.5, 5.0], 105: [5.0, 14.5, 5.0], 106: [-5.0, 14.5, 5.0], 107: [7.0, 0.0, 5.0], 108: [7.0, -7.5, 5.0], 109: [7.0, 7.5, 5.0], 110: [-7.0, 0.0, 5.0], 111: [-7.0, -7.5, 5.0], 112: [-7.0, 7.5, 5.0], } # Number of propagate steps self.N = 5 #Constants self.g = 9.8 # ROS Stuff # Init subscribers self.truth_sub_ = rospy.Subscriber( '/slammer/ground_truth/odometry/NED', Odometry, self.truth_callback) self.imu_sub_ = rospy.Subscriber('/slammer/imu/data', Imu, self.imu_callback) self.aruco_sub = rospy.Subscriber('/aruco/measurements', MarkerMeasurementArray, self.aruco_meas_callback) # Init publishers self.estimate_pub_ = rospy.Publisher('/ekf_estimate', Odometry, queue_size=10) # # Init Timer self.pub_rate_ = 100. # self.update_timer_ = rospy.Timer(rospy.Duration(1.0 / self.pub_rate_), self.pub_est)
def _on_fire_alarm_recv(self, msg, who: str): if self.supervision_state == SupervisorNodeState.Alarm_recv: self.timer = rospy.Timer(rospy.Duration(secs=0, nsecs=500000000), self.timer_callback, oneshot=True)
def on_saop_plan(self, msg: Plan): if self.supervision_state == SupervisorNodeState.Planning: self.timer = rospy.Timer(rospy.Duration(secs=0, nsecs=500000000), self.timer_callback, oneshot=True)
def __init__(self): self.axis_for_right = float(rospy.get_param( '~axis_for_right', 0)) # if right calibrates first, this should be 0, else 1 self.wheel_track = float(rospy.get_param( '~wheel_track', 0.285)) # m, distance between wheel centres self.tyre_circumference = float( rospy.get_param('~tyre_circumference', 0.341) ) # used to translate velocity commands in m/s into motor rpm self.connect_on_startup = rospy.get_param('~connect_on_startup', False) self.calibrate_on_startup = rospy.get_param('~calibrate_on_startup', False) self.engage_on_startup = rospy.get_param('~engage_on_startup', False) self.max_speed = rospy.get_param('~max_speed', 0.5) self.max_angular = rospy.get_param('~max_angular', 1.0) self.publish_current = rospy.get_param('~publish_current', True) self.has_preroll = rospy.get_param('~use_preroll', True) self.publish_current = rospy.get_param('~publish_current', True) self.publish_odom = rospy.get_param('~publish_odom', True) self.publish_tf = rospy.get_param('~publish_odom_tf', False) self.odom_topic = rospy.get_param('~odom_topic', "odom") self.odom_frame = rospy.get_param('~odom_frame', "odom") self.base_frame = rospy.get_param('~base_frame', "base_link") self.odom_calc_hz = rospy.get_param('~odom_calc_hz', 20) rospy.on_shutdown(self.terminate) rospy.Service('connect_driver', std_srvs.srv.Trigger, self.connect_driver) rospy.Service('disconnect_driver', std_srvs.srv.Trigger, self.disconnect_driver) rospy.Service('calibrate_motors', std_srvs.srv.Trigger, self.calibrate_motor) rospy.Service('engage_motors', std_srvs.srv.Trigger, self.engage_motor) rospy.Service('release_motors', std_srvs.srv.Trigger, self.release_motor) self.vel_subscribe = rospy.Subscriber("/cmd_vel", Twist, self.cmd_vel_callback, queue_size=2) self.timer = rospy.Timer( rospy.Duration(0.1), self.timer_check) # stop motors if no cmd_vel received > 1second if self.publish_current: self.current_loop_count = 0 self.left_current_accumulator = 0.0 self.right_current_accumulator = 0.0 self.current_timer = rospy.Timer( rospy.Duration(0.05), self.timer_current ) # publish motor currents at 10Hz, read at 50Hz self.current_publisher_left = rospy.Publisher( 'odrive/left_current', Float64, queue_size=2) self.current_publisher_right = rospy.Publisher( 'odrive/right_current', Float64, queue_size=2) rospy.loginfo("ODrive will publish motor currents.") if self.publish_odom: rospy.Service('reset_odometry', std_srvs.srv.Trigger, self.reset_odometry) self.odom_publisher = rospy.Publisher(self.odom_topic, Odometry, queue_size=2) # setup message self.odom_msg = Odometry() #print(dir(self.odom_msg)) self.odom_msg.header.frame_id = self.odom_frame self.odom_msg.child_frame_id = self.base_frame self.odom_msg.pose.pose.position.z = 0.0 # always on the ground, we hope self.odom_msg.pose.pose.orientation.x = 0.0 # always vertical self.odom_msg.pose.pose.orientation.y = 0.0 # always vertical self.odom_msg.twist.twist.linear.y = 0.0 # no sideways self.odom_msg.twist.twist.linear.z = 0.0 # or upwards... only forward self.odom_msg.twist.twist.angular.x = 0.0 # or roll self.odom_msg.twist.twist.angular.y = 0.0 # or pitch... only yaw # store current location to be updated. self.x = 0.0 self.y = 0.0 self.theta = 0.0 # setup transform self.tf_publisher = tf2_ros.TransformBroadcaster() self.tf_msg = TransformStamped() self.tf_msg.header.frame_id = self.odom_frame self.tf_msg.child_frame_id = self.base_frame self.tf_msg.transform.translation.z = 0.0 self.tf_msg.transform.rotation.x = 0.0 self.tf_msg.transform.rotation.y = 0.0 self.odom_timer = rospy.Timer( rospy.Duration(1 / float(self.odom_calc_hz)), self.timer_odometry) if not self.connect_on_startup: rospy.loginfo("ODrive node started, but not connected.") return if not self.connect_driver(None)[0]: return # Failed to connect if not self.calibrate_on_startup: rospy.loginfo("ODrive node started and connected. Not calibrated.") return if not self.calibrate_motor(None)[0]: return if not self.engage_on_startup: rospy.loginfo("ODrive connected and configured. Engage to drive.") return if not self.engage_motor(None)[0]: return rospy.loginfo("ODrive connected and configured. Ready to drive.")
rotate_control.name = "move_y" rotate_control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS # add the control to the interactive marker int_marker.controls.append(rotate_control); # add the interactive marker to our collection & # tell the server to call processFeedback() when feedback arrives for it server.insert(int_marker, processFeedback) if __name__=="__main__": rospy.init_node("basic_controls") br = TransformBroadcaster() # create a timer to update the published transforms rospy.Timer(rospy.Duration(0.01), frameCallback) server = InteractiveMarkerServer("basic_controls") menu_handler.insert( "First Entry", callback=processFeedback ) menu_handler.insert( "Second Entry", callback=processFeedback ) sub_menu_handle = menu_handler.insert( "Submenu" ) menu_handler.insert( "First Entry", parent=sub_menu_handle, callback=processFeedback ) menu_handler.insert( "Second Entry", parent=sub_menu_handle, callback=processFeedback ) # Adjust yaw ''' position = Point(-3, 3, 0) make6DofMarker( False, InteractiveMarkerControl.NONE, position, True) position = Point( 0, 3, 0)
def main(): update_timer = rospy.Timer(rospy.Duration(10), update_vehicle_list)
def __init__(self): self.rate = rospy.Rate(2) self.task_number = 0 self.error_step = 0 # Tablet data self.object_name = "" self.video_step_url = "" self.video_full_url = "" self.watch_video_url = "" self.mac_address = get_mac() self.is_goto_active = False self.goto_success = False self.goto_type = None self.is_error_correction_done = False self.is_error_corrected = False self.use_robot = True self.use_tablet = True self.teleop_only = False self.is_robot_moving = False self.test = True if rospy.has_param("ras"): ras = rospy.get_param("ras") self.use_robot = ras['use_robot'] self.use_tablet = ras['use_tablet'] self.teleop_only = ras['teleop_only'] self.test = ras['is_test'] self.test_error = False self.use_location = False if rospy.has_param("adl"): adl = rospy.get_param("adl") self.test_error = adl['test_error'] self.use_location = adl['use_location'] self.do_error = SimpleActionServer( 'do_error', DoErrorAction, execute_cb=self.do_error_execute, auto_start=False) # Called from the tablet when we want to go to a particular object # This then forwards it to our Go To node via the self.goto_client if self.use_tablet: self.tablet = SimpleActionServer( 'tablet_response', TabletAction, execute_cb=self.tablet_execute, auto_start=False) self.tablet.start() if self.use_robot: # Forward commands through our Go To node self.goto_client = SimpleActionClient( 'goto', GotoAction) self.goto_client.wait_for_server() # Allow returning to base from the experimenter interface self.goto_base = SimpleActionServer( 'goto_base', TabletAction, # Same as action, but ignore data in message execute_cb=self.goto_base_execute, auto_start=False) self.goto_base.start() self.is_robot_moving = False self.do_error.start() rospy.on_shutdown(self.shutdown) # Initiates CASAS logger rospy.Timer(rospy.Duration(0.01), self.casas_logging, oneshot=True) # Log system information to CASAS rospy.Timer(rospy.Duration(1), self.system_log, oneshot=True) if self.use_robot or self.teleop_only: rospy.Subscriber('/cmd_vel', Twist, self.robot_cmd_vel_cb) self.battery_voltage = None rospy.Subscriber('/sensor_state', SensorState, self.robot_sensor_state_cb) rospy.Timer(rospy.Duration(2), self.robot_battery_cb, oneshot=True) self.tf = TransformListener() rospy.Timer(rospy.Duration(2), self.robot_location_cb, oneshot=True)
rospy.logwarn("No odom data received!Exit!") return True else: return False if __name__ == "__main__": rospy.init_node("DrawingTraj") de = DrawTraj() # gazebo's odom, without nosie rospy.Subscriber("odom", Odometry, de.odomCallBack) # rf2o_odometry's odom rospy.Subscriber("odom_rf2o", Odometry, de.rf2oCallBack) rospy.sleep(2) rospy.Timer(rospy.Duration(3), de.cbMonitor) while not rospy.is_shutdown(): if de.cbMonitor(plt) == True: break ax1 = plt.subplot(1, 2, 1) plt.title("odom display") plt.plot(de.x_odom, de.y_odom, color="b", label="/imu_traj") plt.plot(de.x_rf2o, de.y_rf2o, color="r", label="/rf2o_traj") plt.legend() # down-sampling for gazebo's /odom rate = len(de.y_odom) / len(de.y_rf2o) print rate, len(de.x_rf2o) ax2 = plt.subplot(1, 2, 2) errs = []
host.ip = socket.gethostbyname(host.hostname) # If the host is pingable, mark it as online try: subprocess.check_output(["ping", "-c1", host.ip]) host.status = "Online" # If pinging the host is unsuccessful, mark it as offline except: host.status = "Offline" # If hostname resolution fails, the IP address is set the unknown except: host.ip = "Unknown" host.status = "Unknown" self.hosts.hosts.append(host) def publish(self, event): ''' Publishes the list of hosts and the information gathered about them. ''' self.check_hosts() self.pub_hosts.publish(self.hosts) if __name__ == "__main__": monitor = HostMonitor() rospy.Timer(rospy.Duration(10), monitor.publish, oneshot=False) rospy.spin()
def object_detector_cb(self, msg): ''' Callback object_detector_cb (rostopic pub /object_detector_mqtt_msg decenter_msgs/ObjectDetector ) ''' # rospy.logdebug( rospy.loginfo('object_detector_cb received:: %s' % msg) #if there are not objects return if len(msg.objects) <= 0: rospy.logwarn('object_detector_cb::objects is empty') return robot_data = filter( lambda obj: obj.get('id') == int(msg.metadata.robot_id), self.robots) if not self.send_pictures_enabled: rospy.logwarn('Sending picture is not enabled, doing nothing') return robot_enable_service = robot_data[0]['enable_service'] self.robot_enable_service = robot_enable_service self.enable_send_pictures(enable=False, service=robot_enable_service) #so far we just take the first object detected object_type = msg.objects[0].warehouse_obj_class rospy.logdebug('Identified the object: %s' % object_type) #switch between 3 cases: 'other', 'robot', 'person' if object_type == 'person': rospy.loginfo('Person Detected !') self.send_person_alert(int(msg.metadata.robot_id)) elif object_type == 'robot': rospy.loginfo('Robot Detected !') if not self.send_robot_indicator(int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return #so far the node to disable is hardcoded but parametrized if not self.get_current_mission(int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return if not self.cancel_mission(int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return rospy.sleep(5) if not self.enable_node(self.node_selected): self.object_detector_fail(robot_enable_service) return if not self.unblock_node(node=self.node_selected, robot_id=int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return if not self.disable_node(self.node_selected): self.object_detector_fail(robot_enable_service) return if not self.insert_last_mission(): self.object_detector_fail(robot_enable_service) return if not self.wait_until_robot_takes_new_mission( int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return rospy.Timer(period=rospy.Duration(self.node_enable_wait_time), callback=self.node_enable_timer_callback, oneshot=True) elif object_type == 'others': rospy.loginfo('Other thing Detected') if not self.send_others_indicator(int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return #so far the node to disable is hardcoded but parametrized if not self.get_current_mission(int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return if not self.cancel_mission(int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return rospy.sleep(5) if not self.enable_node(self.node_selected): self.object_detector_fail(robot_enable_service) return if not self.unblock_node(node=self.node_selected, robot_id=int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return if not self.disable_node(self.node_selected): self.object_detector_fail(robot_enable_service) return if not self.insert_last_mission(): self.object_detector_fail(robot_enable_service) return if not self.wait_until_robot_takes_new_mission( int(msg.metadata.robot_id)): self.object_detector_fail(robot_enable_service) return else: rospy.logwarn( 'Indeterminate case. Not person, not robot, not others') self.enable_send_pictures(enable=True, service=robot_enable_service) return # Success rospy.loginfo('Succeed') rospy.Timer(period=rospy.Duration(self.img_enable_wait_time), callback=self.enable_image_timer_callback, oneshot=True) # rospy.sleep(30) # self.enable_send_pictures( # enable=True, # service=robot_enable_service # ) return
def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = 'base_link' self.switch_thred = 1.5 # change to next lane if reach next self.pursuit = PurePursuit() self.lka = rospy.get_param("~lookahead", 0.5) self.pursuit.set_look_ahead_distance(self.lka) ## node path while not rospy.has_param("/guid_path") and not rospy.is_shutdown(): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.tag_ang_init = rospy.get_param("/guid_path_ang_init") self.last_node = -1 self.next_node_id = None self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False self.get_goal = True self.joint_ang = None ## set first tracking lane self.pub_robot_speech = rospy.Publisher("speech_case", String, queue_size=1) self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1) rospy.sleep(1) # wait for the publisher to be set well self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_position", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) sub_fr = rospy.Subscriber('front_right/ranges', DeviceRangeArray, self.cb_range, queue_size=1) sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states', JointState, self.cb_joint, queue_size=1) #Don't update goal too often self.last_update_goal = None self.goal_update_thred = 0.001 self.hist_goal = np.array([]) self.normal = True self.get_goal = True self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking) # print("init done") # prevent sudden stop self.last_plan = None # keep searching until find path or reach search end self.search_angle = 10 * math.pi / 180 self.search_max = 5 ### stupid range drawing # for using tf to draw range br1 = tf2_ros.StaticTransformBroadcaster() br2 = tf2_ros.StaticTransformBroadcaster() # stf.header.frame_id = "uwb_back_left" # stf.child_frame_id = "base_link" # stf.transform.translation.x = -1*r_tag_points[0, 0] # stf.transform.translation.y = -1*r_tag_points[0, 1] # br1.sendTransform(stf) # stf2 = stf # stf2.header.stamp = rospy.Time.now() # stf2.header.frame_id = "uwb_front_right" # stf2.transform.translation.x = -1*r_tag_points[1, 0] # stf2.transform.translation.y = -1*r_tag_points[1, 1] # br2.sendTransform(stf2) stf = TransformStamped() stf.header.stamp = rospy.Time.now() stf.transform.rotation.w = 1 stf.header.frame_id = "base_link" stf.child_frame_id = "uwb_back_left" stf.transform.translation.x = r_tag_points[0, 0] stf.transform.translation.y = r_tag_points[0, 1] stf2 = TransformStamped() stf2.header.stamp = rospy.Time.now() stf2.transform.rotation.w = 1 stf2.header.frame_id = "base_link" stf2.child_frame_id = "uwb_front_right" stf2.transform.translation.x = r_tag_points[1, 0] stf2.transform.translation.y = r_tag_points[1, 1]
def launch_simulation(self): rospy.loginfo( "Experiment setup: waiting for unreal MAV simulation to setup...") # Wait for unreal simulation to setup if self.startup_timeout > 0.0: try: rospy.wait_for_message("unreal_simulation_ready", String, self.startup_timeout) except rospy.ROSException: self.stop_experiment( "Simulation startup failed (timeout after " + str(self.startup_timeout) + "s).") return else: rospy.wait_for_message("unreal_simulation_ready", String) rospy.loginfo("Waiting for unreal MAV simulation to setup... done.") # Launch planner (by service, every planner needs to advertise this service when ready) rospy.loginfo("Waiting for planner to be ready...") if self.startup_timeout > 0.0: try: rospy.wait_for_service(self.ns_planner + "/toggle_running", self.startup_timeout) except rospy.ROSException: self.stop_experiment("Planner startup failed (timeout after " + str(self.startup_timeout) + "s).") return else: rospy.wait_for_service(self.ns_planner + "/toggle_running") if self.planner_delay > 0: rospy.loginfo( "Waiting for planner to be ready... done. Launch in %d seconds.", self.planner_delay) rospy.sleep(self.planner_delay) else: rospy.loginfo("Waiting for planner to be ready... done.") run_planner_srv = rospy.ServiceProxy( self.ns_planner + "/toggle_running", SetBool) run_planner_srv(True) # Setup first measurements self.eval_walltime_0 = time.time() self.eval_rostime_0 = rospy.get_time() # Evaluation init if self.evaluate: self.writelog("Succesfully started the simulation.") # Dump complete rosparams for reference subprocess.check_call([ "rosparam", "dump", os.path.join(self.eval_directory, "rosparams.yaml"), "/" ]) self.writelog("Dumped the parameter server into 'rosparams.yaml'.") self.eval_n_maps = 0 self.eval_n_pointclouds = 1 # Keep track of the (most recent) rosbag bag_expr = re.compile( 'tmp_bag_\d{4}-\d{2}-\d{2}-\d{2}-\d{2}-\d{2}\.bag.' ) # Default names bags = [ b for b in os.listdir( os.path.join(os.path.dirname(self.eval_directory), "tmp_bags")) if bag_expr.match(b) ] bags.sort(reverse=True) if len(bags) > 0: self.writelog("Registered '%s' as bag for this simulation." % bags[0]) self.eval_log_file.write("[FLAG] Rosbag: %s\n" % bags[0].split('.')[0]) else: rospy.logwarn("No tmpbag found. Is rosbag recording?") # Periodic evaluation (call once for initial measurement) self.eval_callback(None) rospy.Timer(rospy.Duration(self.eval_frequency), self.eval_callback) # Finish rospy.loginfo("\n" + "*" * 39 + "\n* Succesfully started the simulation! *\n" + "*" * 39)
def left_cb(self, msg): self.mission_timer = rospy.Timer(rospy.Duration(2.5), self.run_left, True) return {"success": True, "message": ""}
def __init__(self): self.dT = 0.005; self.timenow = time.time() self.oldtime = self.timenow self.timenow = rospy.Time.now() # publish the action and direction self.FSM_action = rospy.Publisher('/action', String, self.actioncallback) self.FSM_direction = rospy.Publisher('/direction', String, self.directioncallback) # create loop rospy.Timer(rospy.Duration(self.dT), self.loop, oneshot=False) self.action = 'stand' self.direction = 'left' self.Wait = 1 self.Back = 0 self.TRight = 0 self.SLeft = 0 self.Forward = 0 self.TLeft = 0 self.SRight = 0 self.T0_EN = 0 self.T0 = 0 self.T1_EN = 0 self.T1 = 0 self.wait_time0 = 1 self.timing_time0 = 0 self.A_time0 = 0 self.B_time0 = 0 self.C_time0 = 0 self.D_time0 = 0 self.delta_t0 = 0 self.Start_time0 = 0 self.wait_time1 = 1 self.timing_time1 = 0 self.A_time1 = 0 self.B_time1 = 0 self.C_time1 = 0 self.D_time1 = 0 self.delta_t1 = 0 self.Start_time1 = 0 self.A = 0 self.B = 0 self.C = 0 self.D = 0 self.E = 0 self.F = 0 self.G = 0 self.H = 0 self.I = 0 self.J = 0 self.K = 0 self.L = 0 self.M = 0 self.N = 0
imu_msg.angular_velocity.x = gyro_x * 0.0174 imu_msg.angular_velocity.y = gyro_y * 0.0174 imu_msg.angular_velocity.z = gyro_z * 0.0174 imu_msg.header.stamp = rospy.Time.now() imu_pub.publish(imu_msg) temp_pub = None imu_pub = None if __name__ == '__main__': rospy.init_node('imu_node') bus = smbus.SMBus(rospy.get_param('~bus', 1)) ADDR = rospy.get_param('~device_address', 0x68) if type(ADDR) == str: ADDR = int(ADDR, 16) IMU_FRAME = rospy.get_param('~imu_frame', 'imu_link') bus.write_byte_data(ADDR, PWR_MGMT_1, 0) temp_pub = rospy.Publisher('temperature', Temperature, queue_size=10) imu_pub = rospy.Publisher('imu/data_raw', Imu, queue_size=10) imu_timer = rospy.Timer(rospy.Duration(0.02), publish_imu) temp_timer = rospy.Timer(rospy.Duration(10), publish_temp) rospy.spin()
#!/usr/bin/env python import rospy import tf rospy.init_node('FBM2_mock') rospy.loginfo('\nFBM2 helper: emitting tf testbed_origin->robot_markerset @120Hz where the robot is at (0,0) aligned with the origin.') # Emit transform continuously: robot_markerset to testbed_origin tf_broadcaster = tf.TransformBroadcaster() def tf_callback(event): tf_broadcaster.sendTransform( (0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "robot_markerset", "testbed_origin" ) rospy.Timer(rospy.Duration(1.0/120.0), tf_callback) # 120Hz, like MoCap rospy.spin()
def __init__(self): self.sub = rospy.Subscriber("joy", Joy, self.callback) self.pub = rospy.Publisher('fusion', Control, queue_size=1) self.last_published_time = rospy.get_rostime() self.last_published = None self.timer = rospy.Timer(rospy.Duration(1. / 20.), self.timer_callback)
#!/usr/bin/env python import rospy from M02_RCDP_sangukbo import util from std_msgs.msg import String # Initialize the node with rospy rospy.init_node('publisher_node') # Create publisher publisher = rospy.Publisher("~topic", String, queue_size=1) # Define Timer callback def callback(event): msg = String() msg.data = "%s is %s!" % (util.getName(), util.getStatus()) publisher.publish(msg) # Read parameter pub_period = rospy.get_param("~pub_period", 1.0) # Create timer rospy.Timer(rospy.Duration.from_sec(pub_period), callback) # spin to keep the script for exiting rospy.spin()
def __init__(self, args): if len(args) == 0: self.motors_lin_vel_scale = 0.5 self.motors_ang_vel_scale = 0.5 self.motors_cmd_topic = "joyop/cmd_vel" elif len(args) == 1: self.motors_lin_vel_scale = [-float(args[0]), float(args[0])] self.motors_ang_vel_scale = [-float(args[0]), float(args[0])] self.cmd_topic = "joyop/cmd_vel" elif len(args) == 2: self.motors_lin_vel_scale = [-float(args[0]), float(args[0])] self.motors_ang_vel_scale = [-float(args[1]), float(args[1])] self.cmd_topic = "joyop/cmd_vel" elif len(args) == 3: self.motors_lin_vel_scale = [-float(args[0]), float(args[0])] self.motors_ang_vel_scale = [-float(args[1]), float(args[1])] self.cmd_topic = args[2] else: rospy.logerr("Too many arguments! Expected 3 arguments at most (" "linear_velocity_scale, angular_velocity_scale and " "cmd_topic)") exit(-1) self.lac_scale = 0.14 self.xtion_yaw_range = [-0.7, 0.7] self.xtion_pitch_range = [-0.45, 0.75] self.picam_yaw_range = [-0.7, 0.7] self.picam_pitch_range = [-0.6, 0.75] self.motors_lin_vel = 0 self.motors_ang_vel = 0 self.angular_ang_vel = 0 self.lac_position = 0 self.xtion_yaw = 0 self.xtion_pitch = 0 self.picam_yaw = 0 self.picam_pitch = 0 self.motors_vel_pub = rospy.Publisher(self.motors_cmd_topic, Twist, queue_size=1) self.lac_position_pub = rospy.Publisher('/linear_actuator/command', Float64, queue_size=1) self.xtion_yaw_pub = rospy.Publisher('/kinect_yaw_controller/command', Float64, queue_size=1) self.xtion_pitch_pub = rospy.Publisher( '/kinect_pitch_controller/command', Float64, queue_size=1) self.picam_yaw_pub = rospy.Publisher('/camera_effector/pan_command', Float64, queue_size=1) self.picam_pitch_pub = rospy.Publisher('/camera_effector/tilt_command', Float64, queue_size=1) joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback) rospy.Timer(rospy.Duration(0.1), self.launch_joy_node, oneshot=True) rospy.sleep(rospy.Duration(3)) rospy.Timer(rospy.Duration(1.0 / 5.0), self.pub_callback, oneshot=False) rospy.spin()
def init(self): rospy.init_node('hydrus_tracking_controller', anonymous=True) self.__hydrus_controller_freq = rospy.get_param('~hydrus_controller_freq', 100.0) self.__control_mode = rospy.get_param('~control_model', POS_VEL) self.__mpc_flag = rospy.get_param('~mpc', False) self.__mpc_horizon = rospy.get_param('~mpc_horizon', 1.0) if self.__mpc_flag: rospy.loginfo("[hydrus_tracking_controller] MPC mode") self.__control_mode = MPC self.__hydrus_odom = Odometry() self.__object_odom = Odometry() self.__primitive_params = PrimitiveParams() self.__primitive_recv_flag = False self.__hydrus_init_process = True self.__object_odom_sub = rospy.Subscriber("/vision/object_odom", Odometry, self.__objectOdomCallback) self.__hydrus_odom_sub = rospy.Subscriber("/uav/cog/odom", Odometry, self.__hydrusOdomCallback) self.__primitive_params_sub = rospy.Subscriber("/track/primitive_params", PrimitiveParams, self.__primitiveParamsCallback) self.__hydrus_motion_init_flag_pub = rospy.Publisher('/track/hydrus_motion_init_flag', Empty, queue_size=1) self.__hydrus_start_pub = rospy.Publisher('/teleop_command/start', Empty, queue_size=1) self.__hydrus_takeoff_pub = rospy.Publisher('/teleop_command/takeoff', Empty, queue_size=1) self.__hydrus_nav_cmd_pub = rospy.Publisher("/uav/nav", FlightNav, queue_size=1) self.__hydrus_joints_ctrl_pub = rospy.Publisher("/hydrus/joints_ctrl", JointState, queue_size=1) self.__mpc_stop_flag_pub = rospy.Publisher('/mpc/stop_cmd', Bool, queue_size=1) self.__mpc_target_waypoints_pub = rospy.Publisher('/mpc/target_waypoints', MpcWaypointList, queue_size=1) rospy.sleep(1.0) self.__hydrus_nav_cmd = FlightNav() self.__hydrus_nav_cmd.header.stamp = rospy.Time.now() self.__hydrus_nav_cmd.header.frame_id = "world" self.__hydrus_nav_cmd.control_frame = self.__hydrus_nav_cmd.WORLD_FRAME self.__hydrus_nav_cmd.target = self.__hydrus_nav_cmd.COG self.__hydrus_nav_cmd.pos_xy_nav_mode = self.__hydrus_nav_cmd.POS_MODE self.__hydrus_nav_cmd.target_pos_x = 0.0 self.__hydrus_nav_cmd.target_pos_y = 0.0 self.__hydrus_nav_cmd.pos_z_nav_mode = self.__hydrus_nav_cmd.POS_MODE self.__hydrus_nav_cmd.target_pos_z = 2.0 self.__hydrus_nav_cmd.psi_nav_mode = self.__hydrus_nav_cmd.POS_MODE self.__hydrus_nav_cmd.target_psi = -math.pi / 2.0 # 0.0 self.__hydrus_joints_ctrl_msg = JointState() self.__hydrus_joints_ctrl_msg.name.append("joint1") self.__hydrus_joints_ctrl_msg.name.append("joint2") self.__hydrus_joints_ctrl_msg.name.append("joint3") self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0) self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0) self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0) ## hydrus start and takeoff if self.__hydrus_init_process: rospy.sleep(1.0) self.__hydrus_start_pub.publish(Empty()) rospy.sleep(1.0) self.__hydrus_takeoff_pub.publish(Empty()) rospy.loginfo("Hydrus arming and takeoff command is sent.") rospy.sleep(21.0) rospy.loginfo("Hydrus takeoff finsihed.") self.__hydrus_nav_cmd_pub.publish(self.__hydrus_nav_cmd) rospy.sleep(9.0) rospy.loginfo("Hydrus moved to initial position.") if self.__control_mode == MPC: mpc_stop_msg = Bool() mpc_stop_msg.data = True self.__mpc_stop_flag_pub.publish(mpc_stop_msg) rospy.sleep(0.2) self.__sendMpcTargetOdom([0.0, 0.0, 0.0], self.__mpc_horizon) rospy.sleep(0.2) mpc_stop_msg.data = False self.__mpc_stop_flag_pub.publish(mpc_stop_msg) rospy.loginfo("Start MPC control.") rospy.sleep(0.2) self.__mpc_start_odom = self.__hydrus_odom self.__hydrus_motion_init_flag_pub.publish(Empty()) rospy.Timer(rospy.Duration(1.0 / self.__hydrus_controller_freq), self.__hydrusControllerCallback)
def on_wildfire_prediction(self, msg: PredictedWildfireMap): if self.supervision_state == SupervisorNodeState.Propagation: self.timer = rospy.Timer(rospy.Duration(secs=1, nsecs=0), self.timer_callback, oneshot=True)
def __init__(self): # Flags for debugging and synchronization self.print_robot_pose = False self.have_map = False self.map_token = False self.map_compute = False # Holds the occupancy grid map self.ogm = 0 self.ogm_copy = 0 # Holds the ogm info for copying reasons -- do not change self.ogm_info = 0 # Holds the robot's total path self.robot_trajectory = [] # Holds the coverage information. This has the same size as the ogm # If a cell has the value of 0 it is uncovered # In the opposite case the cell's value will be 100 self.coverage = 0 # Holds the resolution of the occupancy grid map self.resolution = 0.2 # Origin is the translation between the (0,0) of the robot pose and the # (0,0) of the map self.origin = {} self.origin['x'] = 0 self.origin['y'] = 0 # Initialization of robot pose # x,y are in meters # x_px, y_px are in pixels self.robot_pose = {} self.robot_pose['x'] = 0 self.robot_pose['y'] = 0 self.robot_pose['th'] = 0 self.robot_pose['x_px'] = 0 self.robot_pose['y_px'] = 0 # Use tf to read the robot pose self.listener = tf.TransformListener() # Read robot pose with a timer rospy.Timer(rospy.Duration(0.11), self.readRobotPose) # ROS Subscriber to the occupancy grid map ogm_topic = rospy.get_param('ogm_topic') rospy.Subscriber(ogm_topic, OccupancyGrid, self.readMap) # Publisher of the robot trajectory robot_trajectory_topic = rospy.get_param('robot_trajectory_topic') self.robot_trajectory_publisher = rospy.Publisher(robot_trajectory_topic,\ Path, queue_size = 10) # Publisher of the coverage field coverage_pub_topic = rospy.get_param('coverage_pub_topic') self.coverage_publisher = rospy.Publisher(coverage_pub_topic, \ OccupancyGrid, queue_size = 10) # Get the frames from the param file self.map_frame = rospy.get_param('map_frame') self.base_footprint_frame = rospy.get_param('base_footprint_frame')
import rospy from std_msgs.msg import String rospy.init_node('no2') mat_string = String() def subCallBack(msg): global mat_string mat_string = msg def timerCallBack(event): soma = 0 for i in range(len(mat_string.data)): soma = soma + int(mat_string.data[i]) #print('somando matricula . . . ('+mat_string.data + ')') msg_soma = String() msg_soma.data = str(soma) pub.publish(msg_soma) sub = rospy.Subscriber('/no1/mat', String, subCallBack) pub = rospy.Publisher('no2/sum', String, queue_size=1) timer = rospy.Timer(rospy.Duration(1), timerCallBack) rospy.spin()
def __init__(self): self.debug_flag = True self.arduino_flag = True # Init Decoder self.baudrate = rospy.get_param("~baudrate") connect_success = False while (connect_success == False): # try to connect try: self.decoder = Decoder(self.baudrate) self.arduino_decoder = ArduinoDecoder(19200) connect_success = True except Exception as e: print e rospy.sleep(0.1) # Set decoder protocol self.total_length = rospy.get_param("~total_length") self.data_length = rospy.get_param("~data_length") self.data_order = rospy.get_param("~data_order") self.verify_switch = rospy.get_param("~verify_switch") self.ccd_switch = rospy.get_param("~ccd_switch") self.odo_switch = rospy.get_param("~odo_switch") self.ack_codebook = { "left_shoot": 1, "right_shoot": 1, "left_open": 1, "right_open": 1, "left_close": 0, "right_close": 0 } # offset buffer self.x_offset = 0.0 self.y_offset = 0.0 self.theta_offset = 0.0 self.x_buf = 0.0 self.y_buf = 0.0 self.t_buf = 0.0 # setup frequency self.frequency = 5.0 self.Rate = rospy.Rate(self.frequency) self.plot_debug = True self.display_init = False # setup timer self.start_time = rospy.Time.now() self.select = 0 self.ccd_msg = CCD_data() self.odo_buffer = [0.0, 0.0, 0.0] # Save the name of the node self.node_name = rospy.get_name() rospy.loginfo("[%s] Initialzing." % (self.node_name)) rospy.loginfo("[%s]: baudrate: [%s]" % (self.node_name, self.baudrate)) # Setup publishers self.pub_odo_msg = rospy.Publisher("~odo_msg", Pose2DStamped, queue_size=1) self.pub_odo_debug = rospy.Publisher("~odo_debug", PoseStamped, queue_size=1) self.pub_ccd_msg = rospy.Publisher("~ccd_msg", CCD_data, queue_size=1) self.pub_debug = rospy.Publisher("~debug", Joy, queue_size=1) self.pub_ack_left_shoot = rospy.Publisher("~ack_left_shoot", BoolStamped, queue_size=1) self.pub_ack_right_shoot = rospy.Publisher("~ack_right_shoot", BoolStamped, queue_size=1) self.pub_ack_left_open = rospy.Publisher("~ack_left_open", BoolStamped, queue_size=1) self.pub_ack_right_open = rospy.Publisher("~ack_right_open", BoolStamped, queue_size=1) self.pub_delta_y = rospy.Publisher("~delta_y", Int32, queue_size=1) # Setup Service self.srv_offset = rospy.Service("~set_offset", Empty, self.cbSrvOffset) self.lit = rospy.Service("~lit", Empty, self.cbLit) self.die = rospy.Service("~die", Empty, self.cbDie) # Setup serviceproxy self.macro0 = rospy.ServiceProxy('/Robo/task_planner_node/macro0', Empty) self.macro1 = rospy.ServiceProxy('/Robo/task_planner_node/macro1', Empty) self.macro2 = rospy.ServiceProxy('/Robo/task_planner_node/macro2', Empty) # Read parameters self.pub_timestep = self.setupParameter("~pub_timestep", 0.01) # Create a timer that calls the process function every 1.0 second self.debug_timer = rospy.Timer( rospy.Duration.from_sec(self.pub_timestep), self.cbdebug) #rospy.sleep(rospy.Duration.from_sec(1)) rospy.loginfo("[%s] Initialzed." % (self.node_name))
def __init__(self): # Holds the current drone status. self.status = -1 # Proivde services # ---------------- # For now, the ``rospy.wait_for_service`` calls are # disabled -- that way, this program will still run # even if the drone isn't connected. # # `Toggle camera # <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#toggle-camera>`_ toggle_camera = '/ardrone/togglecam' #rospy.wait_for_service(toggle_camera) self.ToggleCamera = rospy.ServiceProxy(toggle_camera, EmptyServiceType) # Set camera channel (see link above). set_camera_channel = '/ardrone/setcamchannel' #rospy.wait_for_service(set_camera_channel) self.SetCamera = rospy.ServiceProxy(set_camera_channel, CamSelect) # `LED animations # <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#led-animations>`_ led_animations = '/ardrone/setledanimation' #rospy.wait_for_service(led_animations) self.SetLedAnimation = rospy.ServiceProxy(led_animations, LedAnim) # 'Flight animations # <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#flight-animations>`_ # Be careful with these! flight_animations = '/ardrone/setflightanimation' #rospy.wait_for_service(flight_animations) self.SetFlightAnimation = rospy.ServiceProxy(flight_animations, FlightAnim) # `Flat trim # <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#flat-trim>`_ flat_trim = '/ardrone/flattrim' #rospy.wait_for_service(flat_trim) self.SetFlatTrim = rospy.ServiceProxy(flat_trim, EmptyServiceType) # `Record to USB stick # <http://ardrone-autonomy.readthedocs.org/en/latest/services.html#record-to-usb-stick>`_ record_usb = '/ardrone/setrecord' #rospy.wait_for_service(record_usb) self.RecordUsb = rospy.ServiceProxy(record_usb, RecordEnable) # Takeoff, land, and reset # ------------------------ # Allow the controller to publish to the # ``/ardrone/takeoff``, ``land`` and ``reset`` # topics. self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=10) self.pubTakeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=10) self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=10) # Velocity # -------- # Allow the controller to publish to the # ``/cmd_vel`` topic and thus control the drone. self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # Subscribe to the ``/ardrone/navdata`` topic, of # message type navdata, and call # ``self.ReceiveNavdata`` when a message is # received. self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata, self._ReceiveNavdata) # Set up regular publishing of control packets. self.command = Twist() self.commandTimer = rospy.Timer( rospy.Duration(self.COMMAND_PERIOD / 1000.0), self._SendCommand) # Shutdown # -------- # Land the drone when we shut down. rospy.on_shutdown(self.SendLand)
def cb_detection(self, msg): # check there is more than 3 detections # sort the detections by id # check detection id 4 5 6 exist if len(msg.detections) >= 3: detections = sorted(msg.detections, key=lambda s: s.id[0]) id_check = 4 for i in range(3): if i != detections[i].id[0]-4: print('missing gate detection') return else: id_check += 1 # calculate tags position relative to car tag_poses = [] for i in range(3): p = detections[i].pose.pose.pose.position t = detections[i].pose.pose.pose.orientation pos = [p.x, p.y, p.z] tran = [t.x, t.y, t.z, t.w] tag_tf = tf.transformations.concatenate_matrices( tf.transformations.translation_matrix(pos), tf.transformations.quaternion_matrix(tran)) tag_position = np.dot(self.camera2foot, tag_tf)[0:3, 3] tag_poses.append(tag_position) tag_poses = np.array(tag_poses) # ICP total_tf = np.identity(4) for i in range(self.iteration): # find best transform in this iteration transform = self.best_fit_transform(self.gate_poses, tag_poses) # accumulate total transform total_tf = np.dot(transform, total_tf) # transform tag_poses for i in range(tag_poses.shape[0]): tag_poses[i] = transform[0:3, 3] + \ np.dot(transform[0:3, 0:3], tag_poses[i]) # transform matrix to quarternion quat = tf.transformations.quaternion_from_matrix(total_tf) quat = quat/np.linalg.norm(quat) # normalization translation = total_tf[0:3, 3] # save tf if translation[0] < 0: self.translations[self.count] = np.array(translation) self.orientations[self.count] = np.array(quat) self.count += 1 self.broadcaster.sendTransform( translation, quat, rospy.Time.now(), "map", "tmp_global") if self.count == MAX_DETECT: # remove outlier using one class SVM self.translations = self.remove_outlier(self.translations) self.orientations = self.remove_outlier(self.orientations) # calculate mean self.trans_mean = np.mean(self.translations, axis=0) self.orien_mean = np.mean(self.orientations, axis=0) self.orien_mean = self.orien_mean / \ np.linalg.norm(self.orien_mean) # normalization rospy.loginfo('%d translation is used' % self.translations.shape[0]) rospy.loginfo('%d orientations is used' % self.orientations.shape[0]) # stop subscriber self.sub_detection.unregister() # start timer to publish tf self.tiemr = rospy.Timer(rospy.Duration(0.02), self.pub_tf)
def start_nodes_check_loop(self): if self.__check_nodes_timer is None: self.__check_nodes_timer = rospy.Timer( rospy.Duration(1. / rospy.get_param("~check_nodes_frequency")), self.__check_vital_nodes_callback)
pub.publish(msg) msg.linear.x = default_v msg.angular.z = move_w pub.publish(msg) def callback(event): print("Rotated 10 sec using python") rospy.signal_shutdown("Rotated 10 secs") call = 0 createDaemon() rospy.init_node('navigation_husky', anonymous=True) if call == 0: rospy.Timer(rospy.Duration(10), callback) cmdmsg = geometry_msgs.msg.Twist() cmdpub = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist, queue_size=10) rospy.Subscriber('odometry/filtered', nav_msgs.msg.Odometry, huskyOdomCallback, (cmdpub, cmdmsg, call)) rospy.spin() else: cmdmsg = geometry_msgs.msg.Twist() cmdpub = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist, queue_size=10) rospy.Subscriber('odometry/filtered', nav_msgs.msg.Odometry, huskyOdomCallback, (cmdpub, cmdmsg, call)) rospy.signal_shutdown()