def publish_bumper(no, state):
if no != NOTHING:
bumper = BumperEvent()
bumper.bumper = no
bumper.state = state
# rospy.loginfo("sent %d,%d" % (bumper.bumper, bumper.state))
pub.publish(bumper)
def scan_cb(self, data):
event = BumperEvent()
event.bumper = event.CENTER
event.state = event.RELEASED
for i in range(0,len(data.ranges), 1):
if data.ranges[i] < 0.4:
# rospy.logerr("data: " + str(data.ranges))
event.state = event.PRESSED
self.pub.publish(event)
def callback_bumper(self,data):
self.bumper = BumperEvent()
self.bumper = data
if self.bumper.state == 1:
self.avoid_obstacle = True
else:
self.avoid_obstacle = False
def __init__(self,robotID = 'nest', experimentDuration = 600, velocity = 0,distance = 0,experimentWaitDuration = 0):
self.robotID = robotID
self.hz = 40
self.experimentWaitDuration = experimentWaitDuration
self.linear_vel = velocity
self.angular_vel = velocity / 1.77 # half of 0.354 diameter
self.experimentDuration = experimentDuration
self.experimentStart = False
self.yaw = 0
self.bumper = BumperEvent()
self.avoid_obstacle = False
self.drd_heading = 0
self.start = True
self.hdg_ctrl_effort = 0
self.hdg_scale = self.angular_vel/100.0 #max_rot_vel/max_ctrl_effort
self.goal = Point()
self.goal.x,self.goal.y = distance,0
self.pose = Point()
# pose = Odometry()
self.pkg_path = '/home/turtlebot/catkin_ws/src/audio_gen/'
def __init__(self):
rospy.init_node('FakeBumper', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.odom_msg_header = Header()
self.odom_list = LinkedList.LinkedList(TIME_WINDOW)
self.odom_filter_counter = 0
self.bumper_msg = BumperEvent()
self.bumper_msg.bumper = 1
self.bumper_msg.state = 1
try:
self.odom_sub = rospy.Subscriber("odom", Odometry,
self.odom_callback)
except Exception as e:
rospy.loginfo("Couldn't Subscribe to /odom")
print(e)
try:
self.bump_pub = rospy.Publisher("mobile_base/events/bumper",
BumperEvent,
queue_size=10)
except Exception as e:
print(e)
while not rospy.is_shutdown():
try:
rospy.spin()
except Exception as e:
rospy.loginfo("Error in FakeBumper")
print(e)
class TestLoop:
curr_odom = Odometry()
des_vel = Twist()
bumper = BumperEvent()
def __init__(self, this_turtlebot):
rospy.init_node('turtlebot_InfiniteWalk', anonymous=True)
vel_pub = rospy.Publisher('/turtlebot'+ str(this_turtlebot + 1) + '/mobile_base/commands/velocity', Twist, queue_size=10)
rospy.Subscriber('/mavros'+ str(this_turtlebot + 1) + '/odom', Odometry, callback=self.odom_cb)
rate = rospy.Rate(10) # Hz
rate.sleep()
while not rospy.is_shutdown():
linearX =(random.random() - 1)
angularZ = (random.random() - 1) * .75
self.des_vel.linear.x = linearX
self.des_vel.angular.z = angularZ
vel_pub.publish(self.des_vel)
rate.sleep()
def odom_cb(self, msg):
self.curr_odom = msg
def bumper_cb(self, msg):
self.bumper = msg
def __init__(self):
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/teleop',
Twist,
queue_size=10)
self.odom_subscriber = rospy.Subscriber('/odom',
Odometry,
self.OdomCallback,
queue_size=10)
self.bumper_subscriber = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent,
self.BumperCallback,
queue_size=10)
self.depth_subscriber = rospy.Subscriber('/scan',
LaserScan,
self.DepthCallback,
queue_size=10)
self.odom = Odometry()
self.bumper = BumperEvent()
self.depth = LaserScan()
self.maxspeed = 0.3
self.minspeed = 0.07
self.minrotate = 0.06
self.mindis = [10, 10, 10, 10, 10, 10, 10, 10]
self.minpos = [
-26.25, -18.75, -11.25, -3.75, 3.75, 11.25, 18.75, 26.25
]
self.avgdis = [10, 10, 10, 10, 10, 10, 10, 10]
self.avgwidth = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
self.absmindis = 10
self.disthres = [0.4, 0.65, 0.9]
self.finalmin = 10
self.finalminpos = 0
def __init__(self, bot_name):
# bot name
self.name = bot_name
# bumper state
self.bumper = BumperEvent()
self.center_bumper = False
self.left_bumper = False
self.right_bumper = False
self.red_score = 0
self.blue_score = 0
self.green_score = 0
# for convert image topic to opencv obj
self.bridge = CvBridge()
# velocity publisher
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# bumper subscrivre
self.bumper_sub = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent, self.bumperCallback)
# camera subscriver
# please uncoment out if you use camera
self.image_sub = rospy.Subscriber('/camera/rgb/image_raw', Image,
self.imageCallback)
def __init__(self, bot_name):
# bot name
self.name = bot_name
# bumper state
self.bumper = BumperEvent()
self.center_bumper = False
self.left_bumper = False
self.right_bumper = False
# for convert image topic to opencv obj
self.bridge = CvBridge()
# velocity publisher
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# bumper subscrivre
self.bumper_sub = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent, self.bumperCallback)
# camera subscriver
# please uncoment out if you use camera
# get rgb and depth image http://answers.ros.org/question/219029/getting-depth-information-from-point-using-python/
self.image_sub = message_filters.Subscriber("camera/rgb/image_raw",
Image)
self.depth_sub = message_filters.Subscriber("camera/depth/image_raw",
Image)
# ApproximateTimeSynchronizer http://docs.ros.org/api/message_filters/html/python/
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.image_sub, self.depth_sub], 1, 0.1)
self.ts.registerCallback(self.imageDepthcallback)
# view gui flag
self.cv_view = True
# red region near position(loc) and distance(val)
self.red_loc = np.zeros((2))
self.red_val = np.array([0.0])
# yellow region near position(loc) and distance(val)
self.yel_loc = np.zeros((2))
self.yel_val = np.array([0.0])
# center position and max value
self.loc_centor_x = np.array([1])
self.loc_max = np.zeros((2))
# average depth
self.ave_val = np.array([0.0])
self.M_val = np.array([0.0])
self.L_val = np.array([0.0])
self.R_val = np.array([0.0])
# step
self.step = np.array([0])
def __init__(self, bot_name):
# bot name
self.name = bot_name
# bumper state
self.bumper = BumperEvent()
self.center_bumper = False
self.left_bumper = False
self.right_bumper = False
# for convert image topic to opencv obj
self.bridge = CvBridge()
# velocity publisher
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# bumper subscrivre
self.bumper_sub = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent, self.bumperCallback)
def __init__(self):
# subscribers
rospy.Subscriber("mobile_base/events/bumper", BumperEvent,
self.bumper_event_callback)
rospy.Subscriber("robot_controller/goal_reached", Bool,
self.goal_reached_callback)
# publishers
self.cmd_vel_pub = rospy.Publisher('/cmd_vel_mux/input/navi',
Twist,
queue_size=10)
self.enable_controller_pub = rospy.Publisher(
'/robot_controller/enable_controller', Bool, queue_size=10)
# TODO: initialize any parameters for your controller
# pose_goal is the target pose of the robot
self.pose_goal = Pose2D()
self.pose_goal.x = rospy.get_param("~pose_goal_x", 5.0)
self.pose_goal.y = rospy.get_param("~pose_goal_y", 5.0)
self.pose_goal.theta = rospy.get_param("~pose_goal_theta", 0.0)
# bumper event records which bumper is pressed
# example usage:
# if self.bumper_event.state == BumperEvent.PRESSED:
# do something
# if self.bumper_event.bumper == BumperEvent.LEFT:
# do something
self.bumper_event = BumperEvent()
# indicates whether the current via point is reached
self.flag_viapoint_reached = False
# valid states
# TODO: add in or change the valid state based on your design of the state machine
self.valid_states = [
"idle", "moving_to_next", "backing_up", "exploring_boundary",
"goal_reached"
]
# set the initial state
self.state = "idle"
def __init__(self):
'''Initializes an object of this class.
The constructor creates a publisher, a twist message.
3 integer variables are created to keep track of where obstacles exist.
3 dictionaries are to keep track of the movement and log messages.'''
global trigger
self.pub = rospy.Publisher('/key_cmd_vel', Twist)
self.qpub = rospy.Publisher('/cmd_vel', Twist)
self.msg = Twist()
self.bumperstate = BumperEvent()
self.cliffstate = CliffEvent()
self.sectorangles = [0, 0, 0, 0, 0]
self.sectorrange = [0, 0, 0, 0, 0]
self.sectorvio = [0, 0, 0, 0, 0]
self.sectorentry = [0, 0, 0, 0, 0]
self.ang = [-0.42, -0.21, 0, 0.21, 0.42]
self.aim = 0
trigger.write("#0 P1500 U1000\r")
playsound("sentrymode.wav", 10)
def callback_bumper(data):
global bumper, avoid_obstacle
bumper = BumperEvent()
bumper = data
if bumper.state == 1 and not turn:
avoid_obstacle = True
ear.stream_start() freq = 500 linear_vel = 0.1 angular_vel = 0.1 yaw = 0 turn_prob = 1.0 / (hz * 10) mu = math.pi sigma = math.pi / 2.0 tolerance = 10 * math.pi / 180 new_heading = 'NaN' turn_amt = 0 turn = False bumper = BumperEvent() avoid_obstacle = False drd_heading = 0 start = True hdg_ctrl_effort = 0 hdg_scale = angular_vel / 10.0 #max_rot_vel/max_ctrl_effort goal = Point() pose = Point() # spiral = [Point(0,0.2,0),Point(0.2,0.2,0),Point(0.2,-0.2,0),Point(-0.2,-0.2,0), # Point(-0.2,0.4,0),Point(0.4,0.4,0),Point(0.4,-0.4,0),Point(-0.4,-0.4,0), # Point(-0.4,0.6,0),Point(0.6,0.6,0),Point(0.6,-0.6,0),Point(-0.6,-0.6,0), # Point(-0.6,0.8,0),Point(0.8,0.8,0),Point(0.8,-0.8,0),Point(-0.8,-0.8,0), # Point(-0.8,1.0,0),Point(1.0,1.0,0),Point(1.0,-1.0,0),Point(-1.0,-1.0,0), # Point(-1.0,1.0,0)]
def __init__(self):
# ros related variables
rospy.init_node('bug2')
# odometry messages
self.odom = Odometry()
self.starting_odom = Odometry()
# bumper event message
self.bumper_msg = BumperEvent()
# twist message
self.twist_msg = Twist()
# robot's position and direction
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# things you might want to consider using
self.bumper_pressed = -1
self.goal_distance = -1
self.tmp_goal_distance = -1
self.tmp_goal_rotation = -1
self.goal_x = 0.0
self.goal_y = 0.0
#distance_to_goal = 0.0
self.status = RobotStatus.STOPPED
self.taskStatus = TaskStatus.NOTASK
self.reached_goal = False
self.hit_point_x = 0.0
self.hit_pint_y = 0.0
# reading the laser data
self.scan = LaserScan()
self.scan_sub = rospy.Subscriber('/scan',
LaserScan,
self.scan_callback,
queue_size=10)
self.bumper_sub = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent,
self.bumper_callback,
queue_size=1)
# subscribe to odometry messages
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_callback,
queue_size=1)
# publisher for twist messages
self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/teleop',
Twist,
queue_size=10)
# loop rate
rate = rospy.Rate(50) # HZ
start_time = rospy.Time.now()
# wait until specific amount of time, until your messeges start to be received by your node
print("Waiting, the node is initializing...")
# set the goal
#self.set_linear_goal(5.0,0.0)
# you might want to define a temporary goal for when the bot face towards a wall
#self.face_to_goal()
self.drive_straight(0.2, 0.5)
rospy.sleep(1)
self.rotate(-45)
"""
while (not rospy.is_shutdown() & (self.goal_distance > min_distance_to_your_goal) ):
# implement bug2 algorithm
# you might want to print your current position and your tmp and actual goal here
#Start Robot, move robot toward goal
if (self.status == RobotStatus.STOPPED) & (self.taskStatus == TaskStatus.NOTASK):
# Go to goal
self.taskStatus = TaskStatus.GOAL_SEEKING
self.drive_straight(.3,self.goal_distance)
#Wall Following
if(Reach_obstacle) & (self.taskStatus != TaskStatus.WALL_FOLLOWING):
self.taskStatus = TaskStatus.WALL_FOLLOWING
#Following Wall, Change back to FACING_GOAL after done.
self.taskStatus = TaskStatus.FACING_GOAL
#Roate Robot to face the goal, then head to goal;
if(self.taskStatus == TaskStatus.FACING_GOAL):
self.face_to_goal()
self.go_to_goal()
#self.cmd_vel.publish(self.twist_msg)
#rate.sleep()
"""
rospy.loginfo("Stopping Turtlebot")
self.cmd_vel.publish(Twist())
def __init__(self):
# initialize the blinkstick
self.vision_led = blinkstick.find_first()
# set initial states
self.state = rospy.get_param("~state_init", "Idle")
self.state_last = ""
self.cmd_state = rospy.get_param("~cmd_state_init", "Idle")
# flag that indicates whether haptic tether is enables
# 0 - Teleoperation
# 1 - Autonomous
# 2 - Haptic Tether
# 3 - Training
self.follower_mode = 3
# timer for state machine
self.cmd_state_timer = rospy.get_time()
self.cmd_state_period = 0.0
# time in following mode
self.dt_following = 0.0
# loop counters
self.stuck_count = 0
self.stuck_backup_count = 0
self.lost_vision_count = 0
self.too_fast_count = 0
self.slow_down_count = 0
self.too_fast_pause_count = 0
self.flag_too_fast_check_pause = False
self.stuck_backup_count_limit = 50
self.stuck_count_limit = rospy.get_param("~stuck_count_limit", 20)
self.lost_vision_count_limit = rospy.get_param(
"~lost_vision_count_limit", 25)
self.too_fast_count_limit_low = rospy.get_param(
"~too_fast_count_limit_low", 25)
self.too_fast_count_limit_high = rospy.get_param(
"~too_fast_count_limit_high", 45)
self.too_fast_pause_count_limit = rospy.get_param(
"~too_fast_pause_count_limit", 65)
self.slow_down_count_limit = rospy.get_param("~slow_down_count_limit",
25)
self.turtlebot_vel_max = rospy.get_param("~turtlebot_vel_max", 0.7)
self.turtlebot_angvel_max = rospy.get_param("~turtlebot_angvel_max",
np.pi)
# a little bit of hysteresis
self.too_fast_threshold_low = rospy.get_param(
"~too_fast_threshold_low", self.turtlebot_vel_max * 1.4)
self.too_fast_threshold_high = rospy.get_param(
"~too_fast_threshold_high", self.turtlebot_vel_max * 1.5)
# desired velocity for publish
self.cmd_vel = Twist()
self.measured_vel = Twist()
# teleoperation parameters
self.tele_vel = Twist()
self.tele_vel.linear.x = rospy.get_param("~default_teleop_vel_linear",
0.3)
self.tele_vel_inc_linear = rospy.get_param(
"~telop_vel_increment_linear", 0.01)
# bumper variable
self.bumper_event = BumperEvent()
# human tracking variables
self.human_pose = Pose2D()
self.human_vel = Vector3()
self.track_status = "Lost"
# human input variables
self.human_input_tilt = Vector3() # roll, pitch, yaw
self.human_input_gesture = -1 # 10 means no gesture input
self.flag_button_pressed = False
self.flag_latch_cmd_vel = False
self.human_input_mode = rospy.get_param("~human_input_mode",
"tilt_control")
# state machine control
self.set_state = -1 # < 0 means no set state command
self.set_cmd_state = -1
# variables for follower control
self.dist_desired = rospy.get_param("~dist_desired_follower", 1.0)
self.kp_linear = rospy.get_param("~kp_linear", 1.0)
self.kd_linear = rospy.get_param("~kd_linear", 0.1)
self.kb_linear = rospy.get_param("~kb_linear", 0.1)
self.kp_angular = rospy.get_param("~kp_angular", 2.0)
self.kd_angular = rospy.get_param("~kd_angular", 0.2)
self.kb_angular = rospy.get_param("~kb_angular", 0.2)
self.dist_range_min = rospy.get_param("~dist_range_min", 0.3)
self.dist_range_max = rospy.get_param("~dist_range_max", 1.9)
self.dist_stuck_resume_max = rospy.get_param("~dist_stuck_resume_max",
1.0)
# variables for tilt control
self.pitch_to_linear_scale = rospy.get_param("~roll_to_linear_scale",
1.0)
self.roll_to_angular_scale = rospy.get_param("~pitch_to_angular_scale",
-2.0)
self.pitch_deadband = rospy.get_param("~pitch_deadband", 0.2)
self.roll_deadband = rospy.get_param("~roll_deadband", 0.2)
self.pitch_offset = rospy.get_param("~pitch_offset", 0.15)
self.roll_offset = rospy.get_param("~roll_offset", 0.1)
self.roll_center_offset = rospy.get_param("~roll_center_offset", 0.0)
self.pitch_center_offset = rospy.get_param("~pitch_center_offset", 0.0)
self.flag_reverse_mapping = rospy.get_param("~reverse_mapping", False)
# variables for the autonomous turning
self.measured_pose = Odometry()
self.pos_init_pre_turning = Pose2D()
self.dist_pre_turning = 0.0
self.angle_turning = np.pi / 2.0
self.angle_turning_goal = 0.0
self.tilt_turning_thresh = 0.75
# turning direction, 0 - no turning, 1 - turning left, 2 - turning right
self.dir_turning = 0
# subscribers to human tracking
self.human_pose_sub = rospy.Subscriber("tracking/human_pos2d", Pose2D,
self.human_track_pose_cb)
self.human_vel_sub = rospy.Subscriber("tracking/human_vel2d", Vector3,
self.human_track_vel_cb)
self.track_status_sub = rospy.Subscriber("tracking/status", String,
self.human_track_status_cb)
self.odom_sub = rospy.Subscriber("odom", Odometry, self.odom_cb)
# subscribers to human input
self.human_input_ort_sub = rospy.Subscriber("human_input/tilt",
Vector3,
self.human_input_tilt_cb)
self.human_input_gesture_sub = rospy.Subscriber(
"human_input/gesture", Int8, self.human_input_gesture_cb)
self.human_input_mode_sub = rospy.Subscriber("human_input/button",
Bool,
self.human_input_mode_cb)
self.button_event_sub = rospy.Subscriber("human_input/button_event",
Int8, self.button_event_cb)
# subscriber to reverse mapping
self.reverse_mapping_sub = rospy.Subscriber(
"human_input/reverse_mapping", Bool, self.reverse_mapping_cb)
# subscriber to state control
self.state_control_sub = rospy.Subscriber("state_control/set_state",
Int8, self.set_state_cb)
self.cmd_state_control_sub = rospy.Subscriber(
"cmd_state_control/set_state", Int8, self.set_cmd_state_cb)
# subscriber to haptic tether control
self.follower_mode_control_sub = rospy.Subscriber(
"state_control/set_follower_mode", Int8,
self.follower_mode_control_cb)
# subscribe to the bumper event
self.bumper_event_sub = rospy.Subscriber("mobile_base/events/bumper",
BumperEvent,
self.bumper_event_cb)
# publisher to robot velocity
self.robot_state_pub = rospy.Publisher("robot_follower_state",
Int8,
queue_size=1)
self.robot_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
self.haptic_msg_pub = rospy.Publisher("haptic_control",
haptic_msg,
queue_size=1)
# publisher to system message
self.sys_msg_pub = rospy.Publisher("sys_message", String, queue_size=1)
def callback_bumper(self, data):
self.bumper = BumperEvent()
self.bumper = data
if self.bumper.state == 1 and not self.turn:
self.bumper_event = True
def __init__(self,
robotID,
poseOffset=(0, 0, 0),
base_prob=0,
prob_multiplier=10,
prob_divisor=10,
qSize=1,
velocity=0.1,
expDuration=600,
goalPose=None,
hear=False,
theta_A=np.Inf,
experimentWaitDuration=0,
worldWidth=5,
worldLength=5,
centredOrigin=True):
print(robotID, poseOffset, base_prob, prob_divisor, prob_multiplier,
qSize, velocity, expDuration, goalPose, hear, theta_A,
experimentWaitDuration, worldLength, worldWidth, centredOrigin)
self.robotID = robotID
self.experimentWaitDuration = experimentWaitDuration
self.experimentStart = False
self.wall = virtual_wall(worldWidth, worldLength, centredOrigin)
self.hz = 40
self.hear = hear
if self.hear is not None:
self.ear = SWHear.SWHear(rate=44100, updatesPerSecond=self.hz)
else:
self.ear = None
self.theta_A = theta_A
random.seed(int(time.time() / (1 + poseOffset[0]))
) #set random seed to current time and based on self.pose
self.linear_vel = velocity
self.angular_vel = velocity * 2 #/ 1.77 # half of 0.354 diameter
self.poseOffset = poseOffset
self.base_prob = base_prob
self.prob_multiplier = prob_multiplier # probability multiplier
self.prob_divisor = prob_divisor
self.sound_intensity_list = [] # list of sound intensity readings
self.qSize = qSize # size of queue before updating sound intensity values (size of queue for average filter)
self.expDuration = expDuration
self.new_comm_signal = False
self.mu = math.pi
self.sigma = math.pi / 2.0
self.tolerance = 10 * math.pi / 180
self.new_heading = 'NaN'
self.turn_amt = 0
self.turn = False
self.reverseBool = False
self.bumper = BumperEvent()
self.avoid_obstacle = False
self.bumper_event = False # update bound_check to initiate obstacle avoidance
self.start = True
self.hdg_ctrl_effort = 0
self.hdg_scale = self.angular_vel / 1000.0 #max_rot_vel/max_ctrl_effort
if goalPose != None:
self.goal = Point(x=goalPose[0], y=goalPose[1], z=0)
self.goalYaw = goalPose[2]
else:
self.goal = None
self.goalYaw = None
self.pose = Point(x=self.poseOffset[0], y=self.poseOffset[1], z=0)
self.yaw = 0 + self.poseOffset[2]
self.drd_heading = self.yaw
self.home = Point(x=0, y=0, z=0)
# self.pose = Odometry()
self.pkg_path = '/home/turtlebot/catkin_ws/src/my_turtle'