err_x = (self.target_point.linear.x - self.state.pose.pose.position.x)
err_y = (self.target_point.linear.y - self.state.pose.pose.position.y)
euler_angle = self.quat2eul(self.state.pose.pose.orientation)
cmd.linear.x = self.k_p_xy * (err_x * math.cos(euler_angle) + err_y * math.sin(euler_angle))
cmd.linear.y = self.k_p_xy * (- err_x * math.sin(euler_angle) + err_y * math.cos(euler_angle))
cmd.linear.z = self.k_p_z * (self.target_point.linear.z - self.state.pose.pose.position.z)
cmd.angular.z = self.k_yaw * (self.target_point.angular.z - euler_angle.z)
self.move_cmd_send(cmd)
if __name__ == "__main__":
rospy.init_node('DroneInterface1')
drone_1 = DroneInterface('/uav1/ground_truth/state', '/A/uav1/velocity_cmd')
sub_target1 = rospy.Subscriber('/uav1/target_point', drone_1.set_target_point)
target = Twist()
target.linear.x = 5
target.linear.y = 4
target.linear.z = 2
target.angular.z = 0
while not rospy.is_shutdown():
drone_1.set_target_point(target)
drone_1.run()
if __name__=="__main__":
rospy.init_node("le_scan")
velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size = 3 )
recebe_scan = rospy.Subscriber("/bumper", UInt8, scaneou)
while not rospy.is_shutdown():
if bumper == 1:
v= -0.10
w = -0.15
elif bumper == 2:
v= -0.10
w = 0.15
elif bumper == 3:
v= 0.10
w = -0.15
elif bumper == 4:
v= 0.10
w = 0.15
if passa:
passa = False
v=0
w=0
else:
v = 0.17
w = 0
velocidade = Twist(Vector3(v, 0, 0), Vector3(0, 0,w))
velocidade_saida.publish(velocidade)
rospy.sleep(0.5)
def dynamics_function(self, particle_states):
# Get current cmd_vel
try:
cmd_vel = rospy.wait_for_message("/cmd_vel", Twist, timeout=0.1)
except: # If nobody is publishing, set the commands to zero
cmd_vel = Twist()
cmd_vel.angular.z = 0
cmd_vel.linear.x = 0
# Time step between updates
delta_t = 1 / self.update_rate
# Calculate change in angle and arc length traversed
#TODO: FIGURE OUT WHY THESE CONSTANTS ARE REQUIRED
delta_theta = cmd_vel.angular.z * delta_t
delta_s = cmd_vel.linear.x * delta_t
#print("delta_x: {} \t delta_y: {} \t delta_theta: {}".format(delta_x, delta_y, delta_theta))
# Init variables to store translation in x and y directions in the robot frame
# delta_y is the forward direction of the robot
# delta_x is the right direction of the robot
# NOTE: This coordinate frame does not coincide with the ROS coordinate frame of the robot
# ROS coordinate frame is rotated 90 degrees counter-clockwise
delta_x_robot = 0.0
delta_y_robot = 0.0
# This is a singularity where the robot only moves forward
# Radius of circle is infinite
if delta_theta == 0:
delta_y_robot = delta_s
else:
# Radius of the circle along which the robot is travelling
r = delta_s / delta_theta
# Next, calculate the translation in the robot frame
delta_x_robot = r * (np.cos(delta_theta) - 1)
delta_y_robot = r * np.sin(delta_theta)
for i in range(particle_states.shape[0]):
# Transform x and y translation in robot frame to world coordinate frame for each particle
# TODO REMOVE HACKY pi/2 fix
delta_x = delta_x_robot * np.cos(
particle_states[i][2] - np.pi /
2) - delta_y_robot * np.sin(particle_states[i][2] - np.pi / 2)
delta_y = delta_x_robot * np.sin(
particle_states[i][2] - np.pi /
2) + delta_y_robot * np.cos(particle_states[i][2] - np.pi / 2)
particle_states[i][0] = particle_states[i][0] + delta_x
particle_states[i][1] = particle_states[i][1] + delta_y
particle_states[i][2] = particle_states[i][2] + delta_theta
# if(i == 0):
# print("delta_x: {} \t delta_y: {} \t delta_theta: {}".format(delta_x_robot, delta_y_robot, delta_theta))
rospy.loginfo("Succesfully completed dynamics function")
# Return updated particles
return particle_states
def set_cmd_vel(self, position):
rospy.loginfo(" goal: [%5.2f, %5.2f]", position.x, position.y)
goal_x = round(position.x, 4)
goal_y = round(position.y, 4)
# Initialize the robot_position variable as a Point type
robot_position = Point()
# Initialize the command
move_cmd = Twist()
rospy.loginfo(" now -> %s", ok)
# Get the starting position values
(robot_position, rotation) = self.get_odom()
if (ok == True):
# Set the movement command to forward motion
move_cmd.linear.x = self.linear_speed
distanceToGoal = sqrt(
pow((goal_x - robot_position.x), 2) +
pow((goal_y - robot_position.y), 2))
while (distanceToGoal >
self.distance_tolerance): #and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Get the current position
(robot_position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distanceToGoal = sqrt(
pow((goal_x - robot_position.x), 2) +
pow((goal_y - robot_position.x), 2))
rospy.loginfo(
" distanceToGoal:[%5.2f], now -> robot:[%5.2f,%5.2f]",
distanceToGoal, robot_position.x, robot_position.y)
# Stop the robot before the rotation
#Stopping our robot after the movement is over
move_cmd.linear.x = 0
move_cmd.angular.z = 0
self.cmd_vel.publish(move_cmd)
#rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = self.angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
goal_angle = atan2(goal_y - robot_position.y,
goal_x - robot_position.x)
while abs(turn_angle + self.angular_tolerance) < abs(
goal_angle): #and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Get the current rotation
(robot_position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def callback_feedback(data):
'''
Applies adaptive PID to velcity input from odom readings and publishes.
:params data [Odometry]
:params output [Twist]
:params plot [Twist]
'''
global active_vel
global tar_vel
global tar_omega
global wheelbase
global error_sum
global error
global error_diff
global output
global i
global flag
global kp
global ki
global kd
global pub
global prev_error
global gear_stat
global acc_thershold
global brake_threshold
global act_velocity
global yp
global yi
global yd
siny = 2.0 * (data.pose.pose.orientation.w * data.pose.pose.orientation.z +
data.pose.pose.orientation.x * data.pose.pose.orientation.y)
cosy = 1.0 - 2.0 * (
data.pose.pose.orientation.y * data.pose.pose.orientation.y +
data.pose.pose.orientation.z * data.pose.pose.orientation.z)
yaw = math.atan2(siny, cosy)
last_recorded_vel = (data.twist.twist.linear.x * math.cos(yaw) +
data.twist.twist.linear.y * math.sin(yaw))
active_vel = last_recorded_vel
plot = Twist()
output = Twist()
error = tar_vel - active_vel
error_sum += error
error_diff = error - prev_error
prev_error = error
if error == 0:
if tar_vel == 0:
output.linear.x = 0
else:
output.linear.x = output.linear.x - 5
# updating kp, ki, kd using MIT rule
kp = kp + yp * error * error
ki = ki + yi * error * error_sum
kd = kd + yd * error * error_diff
rospy.loginfo("kp is : %f", kp)
rospy.loginfo("ki is : %f", ki)
rospy.loginfo("kd is : %f", kd)
# PID on velocity with updated parameters
if error > 0.01:
output.linear.x = (kp * error + ki * error_sum + kd * error_diff)
if error < -0.01:
output.linear.x = ((kp * error + ki * error_sum + kd * error_diff) -
brake_threshold)
plot.linear.x = tar_vel
plot.linear.y = active_vel
plot.linear.z = tar_vel - active_vel # error term
# thresholding the forward velocity
if output.linear.x > 100:
output.linear.x = 100
if output.linear.x < -100:
output.linear.x = -100
# thresholding the angle
output.angular.z = min(30.0, max(-30.0, tar_delta))
rospy.loginfo("linear velocity : %f", plot.linear.y)
rospy.loginfo("target linear velocity : %f", plot.linear.x)
rospy.loginfo("delta : %f", output.angular.z)
prius_pub(output)
pub1.publish(plot)
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
linear_speed = 0.15
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
angular_speed = 0.5
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(1.0)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
def sender():
li = leap_interface.Runner()
li.setDaemon(True)
li.start()
# pub = rospy.Publisher('leapmotion/raw',leap)
pub_ros = rospy.Publisher('leapmotion/data',leapros)
cmd_pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, queue_size=5)
rospy.init_node('leap_pub')
while not rospy.is_shutdown():
hand_direction_ = li.get_hand_direction()
hand_normal_ = li.get_hand_normal()
hand_palm_pos_ = li.get_hand_palmpos()
hand_pitch_ = li.get_hand_pitch()
hand_roll_ = li.get_hand_roll()
hand_yaw_ = li.get_hand_yaw()
is_hand_ = li.get_is_hand()
msg = leapros()
msg.direction.x = hand_direction_[0]
msg.direction.y = hand_direction_[1]
msg.direction.z = hand_direction_[2]
msg.normal.x = hand_normal_[0]
msg.normal.y = hand_normal_[1]
msg.normal.z = hand_normal_[2]
msg.palmpos.x = hand_palm_pos_[0]
msg.palmpos.y = hand_palm_pos_[1]
msg.palmpos.z = hand_palm_pos_[2]
msg.ypr.x = hand_yaw_
msg.ypr.y = hand_pitch_
msg.ypr.z = hand_roll_
moveBindings = {
'n':(1,0),
'ne':(1,-1),
'w':(0,1),
'e':(0,-1),
'nw':(1,1),
's':(-1,0),
'se':(-1,1),
'sw':(-1,-1),
}
#TODO: Adjust movement parameters
if is_hand_ == True:
print "THERE IS A HAND"
if msg.ypr.y < .2 and msg.ypr.y >-.2: #PITCH FOR LINEAR MOVEMENT
print "NO LINEAR MOVEMENT"
x = 0
elif msg.ypr.y > .2:
print "FORWARD"
x = 1
elif msg.ypr.y <-.2:
print "BACKWARD"
x = -1
if msg.ypr.x < .2 and msg.ypr.x >-.2: #ROLL FOR ANGULAR MOVEMENT
print "NO ANGULAR MOVEMENT"
th = 0
elif msg.ypr.x > .2:
print "RIGHT"
th = -1
elif msg.ypr.x <-.2:
print "LEFT"
th = 1
x_move = .1 * x
th_move = .1 * th
twist = Twist()
twist.linear.x = x_move; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th_move
cmd_pub.publish(twist)
else:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
cmd_pub.publish(twist)
# We don't publish native data types, see ROS best practices
# pub.publish(hand_direction=hand_direction_,hand_normal = hand_normal_, hand_palm_pos = hand_palm_pos_, hand_pitch = hand_pitch_, hand_roll = hand_roll_, hand_yaw = hand_yaw_)
pub_ros.publish(msg)
# Save some CPU time, circa 100Hz publishing.
rospy.sleep(0.01)
def _get_twist(self, linear, angular):
twist = Twist()
twist.linear.x = linear
twist.angular.z = angular
return twist
def tracingWall():
global distance
global regions_
msg = Twist()
msg.linear.x = 0.5
return msg
def makeVelMsg(self):
self.msg_vel = Twist()
self.msg_vel.linear.x = self.v
self.msg_vel.angular.z = self.w
return
def shutdown(self):
rospy.loginfo("Stopping TurtleBot")
self.pub_cmd_vel_l.publish(Twist())
self.pub_cmd_vel_r.publish(Twist())
rospy.sleep(1)
def avoid_object(msg):
global control
global incomingVector
k=0.1
moveK=5
size=2
error=False
objectDetected=False
control=False
#Send commands to avoid incoming object trajectory
#rapid move to avoid objects
print "Incoming object!"
if(msg.translation.x==0):
rx=0
else:
rx=1/msg.translation.x*k
if(msg.translation.y==0):
ry=0
else:
ry=1/msg.translation.y*k
rz=0
#check map:
if slamMap is not None:
if(rx<0):
xMax = round((robotX-slamMap.info.origin.position.x+size)/slamMap.info.resolution)
xMin = round((robotX-slamMap.info.origin.position.x-rx*moveK-size)/slamMap.info.resolution)
else:
xMax = round((robotX-slamMap.info.origin.position.x+rx*moveK+size)/slamMap.info.resolution)
xMin = round((robotX-slamMap.info.origin.position.x-size)/slamMap.info.resolution)
if(ry<0):
yMax = round((robotY-slamMap.info.origin.position.y+size)/slamMap.info.resolution)
yMin = round((robotY-slamMap.info.origin.position.y-rx*moveK-size)/slamMap.info.resolution)
else:
yMax = round((robotY-slamMap.info.origin.position.y+rx*moveK+size)/slamMap.info.resolution)
yMin = round((robotY-slamMap.info.origin.position.y-size)/slamMap.info.resolution)
xRob = round((robotX-slamMap.info.origin.position.x)/slamMap.info.resolution)
yRob = round((robotY-slamMap.info.origin.position.y)/slamMap.info.resolution)
print "Robot coordinates: ", robotX, "-", robotY, "-", robotZ
print "Map data: ", slamMap.info.width, "-", slamMap.info.height, "-", slamMap.info.resolution, "-", slamMap.info.origin.position.x, "-", slamMap.info.origin.position.y
print "Checking map: ", xMax, "-", xMin, "-", yMax, "-", yMin, " Robot on map: ", int(xRob), "-", int(yRob)
for y in range(int(yMin),int(yMax),1):
for x in range(int(xMin),int(xMax),1):
index = x+y*slamMap.info.width
if abs(xRob-x)<1 and abs(yRob-y)<1:
if slamMap.data[index] > 90:
error = True
#sys.stdout.write('R')
elif slamMap.data[index] > 90:
## This square is occupied
objectDetected = True
#sys.stdout.write('X')
#elif slamMap.data[index] >= 0:
## This square is unoccupied
#sys.stdout.write(" ")
#else:
#sys.stdout.write('?')
#sys.stdout.write('\n')
motor_command=Twist()
if not error and objectDetected:
print "Object detected but can not rapid move"
rx=0
ry=0
if robotZ<2:
print "at least go up"
rz=-k*5 #go up
else:
print "at least go down"
rz=robotZ*3 #go down
motor_command.linear.x = -rx
motor_command.linear.y = -ry
motor_command.linear.z = -rz
motor_command_publisher.publish(motor_command)
#disable sensor
rospy.sleep(0.6)
#enable sensor
motor_command.linear.x = rx*2
motor_command.linear.y = ry*2
motor_command.linear.z = rz*2
motor_command_publisher.publish(motor_command)
control=True
incomingVector=msg
print(msg)
def follower_node():
rospy.init_node('follower')
global robotX
global robotY
global robotZ
global motor_command_publisher
motor_command_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size = 10)
waypoint_subscriber = rospy.Subscriber("/waypoint_cmd", Transform, waypoint_callback)
action_subscriber = rospy.Subscriber("/incoming", Transform, avoid_object)
map_subscriber = rospy.Subscriber("/map", OccupancyGrid, map_callback, queue_size = 1000)
listener = tf.TransformListener()
step=1
size=0.7
dx=0
dy=0
dz=0
print "Starting, dont forget to enable motors"
delay = rospy.Rate(50.0);
while not rospy.is_shutdown():
motor_command = Twist()
objectDetected=False
error = False
stopped = False
if control:
try:
#grab the latest available transform from the odometry frame (robot's original location - usually the same as the map unless the odometry becomes inaccurate) to the robot's base.
(translationZ,orientationZ) = listener.lookupTransform("/base_link", "/base_footprint", rospy.Time(0));
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
print("EXCEPTION:",e)
#if something goes wrong with this just go to bed for a second or so and wake up hopefully refreshed.
delay.sleep()
continue
try:
#grab the latest available transform from the odometry frame (robot's original location - usually the same as the map unless the odometry becomes inaccurate) to the robot's base.
(translation,orientation) = listener.lookupTransform("/world", "/base_footprint", rospy.Time(0));
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
print("EXCEPTION:",e)
delay.sleep()
continue
#print "\n---------------------\ntranslation: ", translation, "\norientation: ", orientation, "\nwaypoint:\n", waypoint
robotX=translation[0]
robotY=translation[1]
robotZ=translationZ[2]
print "Robot coordinates: ", robotX, "-", robotY, "-", robotZ
if slamMap is not None:
xMax = round((robotX-slamMap.info.origin.position.x+size)/slamMap.info.resolution)
yMax = round((robotY-slamMap.info.origin.position.y+size)/slamMap.info.resolution)
xMin = round((robotX-slamMap.info.origin.position.x-size)/slamMap.info.resolution)
yMin = round((robotY-slamMap.info.origin.position.y-size)/slamMap.info.resolution)
xRob = round((robotX-slamMap.info.origin.position.x)/slamMap.info.resolution)
yRob = round((robotY-slamMap.info.origin.position.y)/slamMap.info.resolution)
print "Map data: ", slamMap.info.width, "-", slamMap.info.height, "-", slamMap.info.resolution, "-", slamMap.info.origin.position.x, "-", slamMap.info.origin.position.y
print "Checking map: ", xMax, "-", xMin, "-", yMax, "-", yMin, " Robot on map: ", int(xRob), "-", int(yRob)
#sys.stdout.write('\n')
for y in range(int(yMin),int(yMax),1):
for x in range(int(xMin),int(xMax),1):
index = x+y*slamMap.info.width
if abs(xRob-x)<1 and abs(yRob-y)<1:
if slamMap.data[index] > 90:
error = True
#sys.stdout.write('R')
elif slamMap.data[index] > 90:
## This square is occupied
objectDetected = True
#sys.stdout.write('X')
#elif slamMap.data[index] >= 0:
## This square is unoccupied
#sys.stdout.write(" ")
#else:
#sys.stdout.write('?')
#sys.stdout.write('\n')
else:
print "Map data not found! Check if slam works"
if not error and objectDetected:
print("Object detected, stop!")
if not stopped and dx!=0 and dy!=0 and dz!=0:
motor_command.linear.x = -dx
motor_command.linear.y = -dy
motor_command.linear.z = -dz
#print "command!\n", motor_command.linear.x, "-", motor_command.linear.y, "-", motor_command.linear.z, "-k: ", k
motor_command_publisher.publish(motor_command)
stopped=True
else:
if(abs(robotZ)<0.3): #prevent collisions
motor_command.linear.z = 0.2
motor_command_publisher.publish(motor_command)
rospy.sleep(0.2)
dx=waypoint.translation.x-robotX
dy=waypoint.translation.y-robotY
dz=waypoint.translation.z+robotZ#base link has negative z
#print "moving!\n", dx, "-", dy, "-", dz
k=1
if(abs(dx)>step or abs(dy)>step or abs(dz)>step):
maxd = max(abs(dx),abs(dy),abs(dz))
k = step / maxd
dx = dx*k
dy = dy*k
dz = dz*k
motor_command.linear.x = dx
motor_command.linear.y = dy
motor_command.linear.z = dz
#print "command!\n", motor_command.linear.x, "-", motor_command.linear.y, "-", motor_command.linear.z, "-k: ", k
motor_command_publisher.publish(motor_command)
def callback_odom(data):
global pose_curr, vel_curr
pose_curr = data.pose.pose
vel_curr = data.twist.twist
pub_twist = rospy.Publisher('cmd_vel', Twist, queue_size=10)
pub_pose = rospy.Publisher('cmd_pose', Pose, queue_size=10)
rospy.init_node('dumb_navigation_planner', anonymous=True)
rospy.Subscriber("tick", Int32, callback_tick)
rospy.Subscriber('odom', Odometry, callback_odom)
# rospy.Subscriber('scan_filtered', LaserScan, callback_scan)
rospy.Subscriber('cloud_filtered', PointCloud2, callback_cloud)
rate = rospy.Rate(30)
val = Twist()
pos = Pose()
pose_curr = Pose()
vel_curr = Twist()
fin = True
ind = -1
start = False
count = 0
chassis_width = rospy.get_param('chassis_width', 1.143)
stop_dist = rospy.get_param('stopping_distance', .5)
obj_thresh = 10
obj_accum = 0
unwrapper = Unwrapper(math.pi)
if rospy.has_param('dumb_plan'):
if target_speed > control_speed:
control_speed = min(target_speed, control_speed + 0.02)
elif target_speed < control_speed:
control_speed = max(target_speed, control_speed - 0.02)
else:
control_speed = target_speed
if target_turn > control_turn:
control_turn = min(target_turn, control_turn + 0.1)
elif target_turn < control_turn:
control_turn = max(target_turn, control_turn - 0.1)
else:
control_turn = target_turn
# 创建并发布twist消息
twist = Twist()
twist.linear.x = control_speed
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = control_turn
pub.publish(twist)
except BaseException as e:
print e
finally:
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
def turningRight():
msg = Twist()
msg.angular.z = -0.25
return msg
front_cone = []
for j in range(355,359):
front_cone.append(msg.ranges[j])
for k in range(0,6):
front_cone.append(msg.ranges[k])
front = min(front_cone)
if __name__ == '__main__':
rospy.init_node('gazebo_scan_sub')
vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 10)
scan_sub = rospy.Subscriber('/scan', LaserScan, callback_scan)
rate = rospy.Rate(10)
vel_msg = Twist()
vel_msg.linear.x = 0.1
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
while not rospy.is_shutdown():
while front < 0.75:
if left_dist == float('inf'): dir = 1
elif right_dist == float('inf'): dir = -1
elif left_dist>right_dist: dir = 1
else: dir = -1
vel_msg.angular.z = dir * 0.75
def turningLeft():
msg = Twist()
msg.angular.z = 0.5
return msg
def newOdom(msg):
global x, y, second_x, second_y, temp1_x, temp2_x, temp1_y, temp2_y
global theta
global pointList
global listnum, listlength
global init_y, init_x
global flag
global r2d
global yaw, yaw_temp
global robot_path
global two_robot_point
global time_flag, data_flag
global start_time
global size_x, arrive_flag, err_dist_list, goal_angle_list, theta_list
global goal2_theta, push_dist, push_flag, other_dist
two_robot_point = [
float("{0:.3f}".format(size_x - x)),
float("{0:.3f}".format(y))
]
robot_path.append(two_robot_point)
speed = Twist()
if flag == 0 and data_flag == 1:
init_x = msg.pose.pose.position.x * r_size_x
init_y = msg.pose.pose.position.y * r_size_y
temp1_x = init_x
temp2_x = init_x
temp1_y = init_y
temp2_y = init_y
flag = 1
elif flag == 1 or flag == 2 or flag == 3:
if time_flag == 0:
start_time = rospy.Time.now()
print("start time : ", start_time)
time_flag = 1
x = msg.pose.pose.position.x * r_size_x + size_x - pointList[0]
y = msg.pose.pose.position.y * r_size_y + pointList[1]
temp2_x = temp1_x
temp1_x = x
x = (temp1_x + temp2_x) / 2.0
temp2_y = temp1_y
temp1_y = y
y = (temp1_y + temp2_y) / 2.0
rot_q = msg.pose.pose.orientation
(roll, pitch,
theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
if (theta > pi):
theta = theta - 2.0 * pi
elif (theta < -pi):
theta = theta + 2.0 * pi
if flag == 1 or flag == 2:
start()
if listnum == listlength and flag == 2:
err_theta = get_last_err_theta(goal2_theta / r2d)
err_theta_d = err_theta * r2d
pid_theta = thetaPID(err_theta)
if ((err_theta_d > 3 and err_theta_d < 360)
or (err_theta_d < -3 and err_theta_d > -360)):
speed.linear.x = 0.0
if (err_theta_d < 0):
speed.angular.z = pid_theta
else:
speed.angular.z = -pid_theta
else:
speed.angular.z = 0.0
finish_time = rospy.Time.now()
print('finish time : ', finish_time)
print((finish_time - start_time) / 1000000000.0)
second_x = x
second_y = y
flag = 3
pub.publish(speed)
elif flag == 3 and push_flag == 1 and arrive_flag == 1:
other_dist = forword_dist(push_dist)
up_down = direction()
if up_down == "foward":
if other_dist <= 1.2:
speed.angular.z = 0.0
speed.linear.x = 0.0
flag = 4
else:
speed.angular.z = 0.0
speed.linear.x = other_dist * 0.01
else:
if other_dist <= 1.2:
speed.angular.z = 0.0
speed.linear.x = 0.0
flag = 4
else:
speed.angular.z = 0.0
speed.linear.x = -1 * other_dist * 0.01
pub.publish(speed)
elif flag == 4:
file = open('/home/dodo/robot2_path.txt', 'w')
for a in range(0, len(robot_path)):
line = "%5.2f %5.2f \n" % (robot_path[a][0], robot_path[a][1])
file.write(line)
file.close()
f = open('/home/dodo/robot2_path.txt', 'r')
lines = f.readlines()
plot_path(pointList, robot_path)
two_robot_point = []
err_dist_list = []
goal_angle_list = []
theta_list = []
pointList = 0
data_flag = 0
listnum = 0
flag = 0
arrive_flag = 0
push_flag = 0
arrive.publish(flag)
def publisher_node(self):
rospy.sleep(2)
twist = Twist()
init_t = rospy.get_time()
rate = rospy.Rate(10)
#Line-following Controller Initialization
desired = 300 # Line index value at which robot is centred
k_p = 0.0057
correction = 0
#Bayesian Localization Parameter Initialization
state_model = [0.05, 0.1, 0.85] # State model for u_k = +1
meas_model = {
0: {
0: 0.6,
1: 0.05,
2: 0.2,
3: 0.05
}, #green
1: {
0: 0.05,
1: 0.6,
2: 0.05,
3: 0.15
}, #orange
2: {
0: 0.2,
1: 0.05,
2: 0.6,
3: 0.05
}, #blue
3: {
0: 0.05,
1: 0.2,
2: 0.05,
3: 0.65
}, #yellow
4: {
0: 0.1,
1: 0.1,
2: 0.1,
3: 0.1
}
} #line
#Measurement_model represented as dictionary with keys as measurement z_k and value being a dictionary with key x_k and corresponding probabilities
pred = np.zeros(12) # Position prediction at each office location
update = np.zeros(
12, dtype=np.float
) # Update to state estimation probabilities based on prediction and measurement model
norm = np.zeros(
12
) # Normalization constant at each update to the state estimation
curr_state = [1 / float(len(pred))] * len(
pred
) # Current state estimation, starting with uniform probability distribution
# Reference RGB colour codes
ref = np.zeros((5, 3))
ref[0] = [133, 163, 1] #green
ref[1] = [222, 48, 1] #orange
ref[2] = [35, 60, 82] #blue
ref[3] = [171, 140, 2] #yellow
ref[4] = [120, 96, 34] #line
j = 0
while rospy.get_time() - init_t < (250):
actual = int(self.line_idx)
colour = None
rgb_error = np.abs(ref - self.measured_rgb)
rgb_sum = np.sum(rgb_error, axis=1)
min_ind = np.argmin(rgb_sum) # Colour estimation
if ((min_ind == 0) or (min_ind == 1) or (min_ind == 2)
or (min_ind == 3)): # If a colour is seen
correction = 0
colour = min_ind
elif (min_ind == 4
): # If no colour measured/seen (detected a line)
error = desired - actual
correction = k_p * error # Correction for P control
twist.angular.z = correction
twist.linear.x = 0.05
self.cmd_pub.publish(twist)
curr_office = np.argmax(curr_state) + 1 # Current state prediction
print(curr_state, (curr_office))
if correction == 0 and colour != None: # If a colour is seen
if curr_office == 4 or curr_office == 6 or curr_office == 8: # If we are at our chosen office location for delivery
init_2 = rospy.get_time()
rospy.sleep(3)
while rospy.get_time() - init_2 < (
(math.pi) / .32): #90 degree left rotation
twist.linear.x = 0
twist.angular.z = 0.2
self.cmd_pub.publish(twist)
rate.sleep()
init_3 = rospy.get_time()
rospy.sleep(1) # Pause
while rospy.get_time() - init_3 < (
(math.pi) / .48): #90 degree right rotation
twist.linear.x = 0
twist.angular.z = -0.2
self.cmd_pub.publish(twist)
rate.sleep()
# Resume straight-line motion
twist.linear.x = 0.05
twist.angular.z = 0
self.cmd_pub.publish(twist)
rospy.sleep(3)
else:
rospy.sleep(6.69) # Continue straight-line motion
for i in range(0, len(curr_state)): # Position prediction step
if (i == 0): # First point in map
step_fwd = curr_state[len(curr_state) -
1] * state_model[2]
if (i != 0):
step_fwd = curr_state[i -
1] * state_model[2] #loop around
if (i == len(curr_state) - 1):
step_bwd = curr_state[0] * state_model[0]
if (i != len(curr_state) - 1):
step_bwd = curr_state[i + 1] * state_model[0]
step_none = curr_state[i] * state_model[1]
pred[i] = (step_fwd + step_bwd + step_none)
for k in range(0, len(pred)): #update based on measurement
update[k] = pred[k] * meas_model[colour][color_map[k]]
norm[j] += update[k]
update = [float(x) / float(norm[j]) for x in update]
curr_state = update # This evolves at each colour measurement
j += 1 # Increment j every time you get a measurement
rate.sleep()
pass
#
recebedor = rospy.Subscriber(topico_imagem,
CompressedImage,
roda_todo_frame,
queue_size=4,
buff_size=2**24)
print("Usando ", topico_imagem)
velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
recebe_scan = rospy.Subscriber("/scan", LaserScan, scaneou)
try:
while not rospy.is_shutdown():
vel = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0))
if dist <= 0.25:
vel = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0))
forward = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0))
t = 0.1
else:
print(dist)
if true == 0:
forward = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0))
t = 0
elif true != 0:
forward = Twist(Vector3(0.1, 0, 0), Vector3(0, 0, 0))
def control_callback(event):
# Actual control is done here. The 'event' is the rospy.Timer() duration period, can be used
# for trubleshooting. To test how often is executed use: $ rostopic hz /mallard/thruster_commands.
global x0,y0,q0,x_goal,y_goal,q_goal,ed
global time_elapsed,t_goal,t_goal_psi
global t_ramp,psivel
twist = Twist()
# Get forces in global frame using PD controller
if(goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0):
# new code - velocity ramp
# get current time
tn = rospy.Time.now()
time_now = tn.secs + (tn.nsecs * 0.000000001)
# time elapsed form the start of tracking the goal (equal to t_now in goal selector)
time_elapsed = time_now - time_begin
# get max velocities
xvelmax = abs(kguseful.safe_div((x_goal-x0),t_goal))
yvelmax = abs(kguseful.safe_div((y_goal-y0),t_goal))
# get desired position,velocity and acceleration from velocity ramp
x_des, x_vel_des, x_acc_des = kglocal.velramp(time_elapsed, xvelmax, x0, x_goal, t_ramp,name="x")
y_des, y_vel_des, y_acc_des = kglocal.velramp(time_elapsed, yvelmax, y0, y_goal, t_ramp,name="y")
# get desired angle:
qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, time_elapsed)
psi_des = tft.euler_from_quaternion(qdes)[2] # Its a list: (roll,pitch,yaw)
psi_vel_des = kglocal.desvelpsi_fun(ed, t_goal_psi, time_elapsed,psivel)
# end new code
# PD global control
x_global_ctrl = control.proportional(x, x_goal, x_vel, x_vel_goal, param['kp'], param['kd'], param['lim'])
y_global_ctrl = control.proportional(y, y_goal, y_vel, y_vel_goal, param['kp'], param['kd'], param['lim'])
psi_global_ctrl = control.proportional_angle(psi, psi_goal,psi_vel,psi_vel_goal, param['kp_psi'], param['kd_psi'], param['lim_psi'])
# Convert to body coordiante
x_PD_body = math.cos(psi)*x_global_ctrl + math.sin(psi)*y_global_ctrl
y_PD_body =-math.sin(psi)*x_global_ctrl + math.cos(psi)*y_global_ctrl
# Publish to twist velocitycommand
twist.linear.x = x_PD_body
twist.linear.y = y_PD_body
twist.angular.z = -psi_global_ctrl
# Generate time instance
now = rospy.Time.now()
twist.angular.x = now.secs
twist.angular.y = now.nsecs
# Publish forces to simulation (joint_state_publisher message)
pub_velocity.publish(twist)
# send [time,position,velocity,goal_position,goal_velocity,control input]
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())
def shutdown(self):
# Always stop the robot when shutting down the node.
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
rospy.sleep(1)
def __init__(self):
#give the node a name
rospy.init_node('calibrate_linear', anonymous=False)
#set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
#How fast will we check the odometry values?
self.rate = 10
r = rospy.Rate(self.rate)
#set the distance to travel
self.test_distance = 1.5
self.speed = 0.2
self.tolerance = 0.01
self.odom_linear_scale_correction = 1.0
self.start_test = True
#Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
#The base frame is base_footprint for the robot
self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
#The odom frame is usually just /odom
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
#initialize the tf listener
self.tf_listener = tf.TransformListener()
#give tf some time to fill its buffer
rospy.sleep(2)
#make sure we see the odom and base frames
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(), rospy.Duration(60.0))
self.position = Point()
#get the starting position from the tf transform between the odom and base frames
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
move_cmd = Twist()
while not rospy.is_shutdown():
#Stop the robot by default
move_cmd = Twist()
if self.start_test:
#get the current position from the tf transform between the odom and base frames
self.position = self.get_position()
#compute the euclidean distance from the target point
distance = sqrt(
pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))
#correct the estimate distance by the correction factor
distance *= self.odom_linear_scale_correction
#How close are we?
error = distance - self.test_distance
#are we close enough?
if not self.start_test or abs(error) < self.tolerance:
self.start_test = False
params = False
rospy.loginfo(params)
else:
#if not, move in the appropriate direction
move_cmd.linear.x = copysign(self.speed, -1 * error)
else:
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
self.cmd_vel.publish(move_cmd)
r.sleep()
#stop the robot
self.cmd_vel.publish(Twist())
def TwistKDLToMsg(kdl_twist):
msg_twist = Twist()
msg_twist.linear = Vector3(*kdl_twist.vel)
msg_twist.angular = Vector3(*kdl_twist.rot)
return msg_twist
def shutdown(self):
twist = Twist()
twist.linear.x = 0
twist.angular.z = 0
self.pub_vel.publish(twist)
print("shutdown")
import roslib
import rospy
roslib.load_manifest('drone')
# Import the messages we're interested in sending and receiving
from geometry_msgs.msg import Twist # for sending commands to the drone
from std_msgs.msg import Float64 # for the control_effort
# Allow the controller to publish to the /cmd_vel topic and thus control the drone
#global variable so we can use it inside the callback
pubCmdTo_drone = rospy.Publisher('drone2/cmd_vel', Twist, queue_size=1)
#global variable because I dont want to reset to zero the previous command in every callback. Otherwise the drone would be stopped position
command = Twist()
def ApplyControlEffort_Yaw(controlEffort):
global command
yaw_velocity=controlEffort.data
command.angular.z = yaw_velocity
pubCmdTo_drone.publish(command)
PrintCommands()
def ApplyControlEffort_Pitch(controlEffort):
global command
pitch = controlEffort.data
#need the minus pitch so it goes the right way
command.linear.x = pitch
pubCmdTo_drone.publish(command)
def execute_cb(self, goal):
rate = rospy.Rate(20)
success = True
# Publish move_base goal marker
quaternion = goal.target_pose.pose.orientation
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.type = 0
marker.action = Marker.ADD
marker.pose.position.x = goal.target_pose.pose.position.x
marker.pose.position.y = goal.target_pose.pose.position.y
marker.pose.position.z = 0
marker.pose.orientation = quaternion
marker.scale.x = 1.0
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.b = 1.0
marker.color.g = 0.0
marker.frame_locked = True
self.goalmarkerPublisher.publish(marker)
pos = Pose2D(self.x, self.y, self.theta)
# Check if PR2 is inside the bounds
if not self.check_bounds(pos, self.HardBound):
rospy.logwarn(
'Trixi is not in the legal area. Please use the joystick to navigate into the legal area.'
)
#self._result.success = False
#self._result.error_message = "Trixi is not in the legal area. Please use the joystick to navigate into the legal area."
self._as.set_aborted(self._result)
return
# Check if the target is inside the bounds
#self._transformer.waitForTransform('map', goal.target_pose.header.frame_id, rospy.Time(), rospy.Duration(2))
goalMap = self._tfBuffer.transform(goal.target_pose, "map")
ignored1, ignored2, yaw = euler_from_quaternion([
goalMap.pose.orientation.x, goalMap.pose.orientation.y,
goalMap.pose.orientation.z, goalMap.pose.orientation.w
])
goal2D = Pose2D(goalMap.pose.position.x, goalMap.pose.position.y, yaw)
if not self.check_bounds(goal2D, self.SoftBound):
rospy.logwarn('Requested point is outside the legal soft bound.')
#self._result.success = False
#self._result.error_message = "Requested point is outside the legal bounds."
self._as.set_aborted(self._result)
return
integral_angle = 0
integral_translation_x = 0
integral_translation_y = 0
while not rospy.is_shutdown():
# Check if PR2 is inside the bounds
pos.updatePose(self.x, self.y, self.theta)
if not self.check_bounds(pos, self.HardBound):
rospy.logwarn(
'Trixi is not in the legal area. Please use the joystick to navigate into the legal area.'
)
#self._result.success = False
#self._result.error_message = "Trixi is not in the legal area. Please use the joystick to navigate into the legal area."
self._as.set_aborted(self._result)
return
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
break
try:
goal.target_pose.header.stamp = rospy.Time(
) #rospy.get_rostime()
goalBaseLink = self._tfBuffer.transform(
goal.target_pose, 'base_footprint')
dir = goalBaseLink.pose.position
ignored1, ignored2, angle_diff = euler_from_quaternion([
goalBaseLink.pose.orientation.x,
goalBaseLink.pose.orientation.y,
goalBaseLink.pose.orientation.z,
goalBaseLink.pose.orientation.w
])
# TODO: fine tune on PR2
# TODO: PI-controller
translation_error = math.sqrt(dir.x**2 + dir.y**2)
integral_angle += angle_diff
integral_translation_x += dir.x
integral_translation_y += dir.y
if not (-0.087 < angle_diff <
0.087): # ca. 5 degrees of tollerance
message = Twist()
max_ang_vel = math.pi / 6
# TODO fine tune on PR2
message.angular.z = clamp(angle_diff * 0.8, -max_ang_vel,
max_ang_vel)
self.publisher.publish(message)
elif not (translation_error < 0.1):
message = Twist()
max_lin_vel = 0.15
# TODO fine tune on PR2
message.linear.x = clamp(dir.x, -max_lin_vel, max_lin_vel)
message.linear.y = clamp(dir.y, -max_lin_vel, max_lin_vel)
self.publisher.publish(message)
else:
break
# TODO: fill out move_base feedback
# should you use the amcl position or just ask tf for (0,0,0) base_footprint transformed to map
self._as.publish_feedback(self._feedback)
# TODO: doublecheck the documentation if these exceptions are still relevant in tf2_ros for the transform method
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print("failed to get transform")
print("[" + str(type(e)) + "]" + str(e))
rate.sleep()
continue
rate.sleep()
if success:
#self._result.success = True
#self._result.error_message = "We successfully reached the specified goal."
rospy.loginfo('Succeeded')
self._as.set_succeeded(self._result)
class Controller(object):
tele_twist = Twist()
def __init__(self, mode, scale_x, scale_z, rate):
self._mode = mode
self._scale_x = scale_x
self._scale_z = scale_z
self._timer = Timer()
self._rate = rospy.Rate(rate)
self._enable_auto_control = False
self.current_control = None
self.trajectory_index = None
self.bridge = CvBridge()
# callback data store
self.image = None
self.left_img = None
self.right_img = None
self.depth_img = None
self.me1_left = None
self.me1_right = None
self.me1_depth = None
self.me2_left = None
self.me2_right = None
self.me2_depth = None
self.me3_left = None
self.me3_right = None
self.me3_depth = None
self.intention = None
self.imu = None
self.imu1 = None
self.imu2 = None
self.imu3 = None
self.odom = None
self.speed = None
self.labeled_control = None
self.key = None
self.training = False
self.scan = None
self.manual_intention = 'forward'
self.is_manual_intention = False
# subscribe ros messages
rospy.Subscriber('/mynteye/left/image_raw/compressed',
CompressedImage,
self.cb_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye/right/image_raw/compressed',
CompressedImage,
self.cb_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye/depth/image_raw/compressed',
CompressedImage,
self.cb_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_1/left/image_raw/compressed',
CompressedImage,
self.cb_me1_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_1/right/image_raw/compressed',
CompressedImage,
self.cb_me1_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_1/depth/image_raw/compressed',
CompressedImage,
self.cb_me1_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_2/left/image_raw/compressed',
CompressedImage,
self.cb_me2_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_2/right/image_raw/compressed',
CompressedImage,
self.cb_me2_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_2/depth/image_raw/compressed',
CompressedImage,
self.cb_me2_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_3/left/image_raw/compressed',
CompressedImage,
self.cb_me3_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_3/right/image_raw/compressed',
CompressedImage,
self.cb_me3_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_3/depth/image_raw/compressed',
CompressedImage,
self.cb_me3_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/scan',
LaserScan,
self.cb_scan,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu',
Imu,
self.cb_imu,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu1',
Imu,
self.cb_imu1,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu2',
Imu,
self.cb_imu2,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu3',
Imu,
self.cb_imu3,
queue_size=1,
buff_size=2**10)
if mode == 'DLM':
rospy.Subscriber('/test_intention',
String,
self.cb_dlm_intention,
queue_size=1)
else:
rospy.Subscriber('/intention_lpe',
Image,
self.cb_lpe_intention,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/speed', Float32, self.cb_speed, queue_size=1)
rospy.Subscriber('/odometry/filtered',
Odometry,
self.cb_odom,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/joy', Joy, self.cb_joy)
# publish control
self.control_pub = rospy.Publisher('/RosAria/cmd_vel',
Twist,
queue_size=1)
# publishing as training data
self.pub_intention = rospy.Publisher('/test_intention',
String,
queue_size=1)
self.pub_trajectory_index = rospy.Publisher('/train/trajectory_index',
String,
queue_size=1)
self.pub_teleop_vel = rospy.Publisher('/train/cmd_vel',
Twist,
queue_size=1)
self.pub_left_img = rospy.Publisher(
'/train/mynteye/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_right_img = rospy.Publisher(
'/train/mynteye/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_depth_img = rospy.Publisher(
'/train/mynteye/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me1_left_img = rospy.Publisher(
'/train/mynteye_1/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me1_right_img = rospy.Publisher(
'/train/mynteye_1/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me1_depth_img = rospy.Publisher(
'/train/mynteye_1/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me2_left_img = rospy.Publisher(
'/train/mynteye_2/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me2_right_img = rospy.Publisher(
'/train/mynteye_2/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me2_depth_img = rospy.Publisher(
'/train/mynteye_2/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me3_left_img = rospy.Publisher(
'/train/mynteye_3/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me3_right_img = rospy.Publisher(
'/train/mynteye_3/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me3_depth_img = rospy.Publisher(
'/train/mynteye_3/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_intention = rospy.Publisher('/train/intention',
String,
queue_size=1)
self.pub_imu = rospy.Publisher('/train/imu', Imu, queue_size=1)
self.pub_imu1 = rospy.Publisher('/train/imu1', Imu, queue_size=1)
self.pub_imu2 = rospy.Publisher('/train/imu2', Imu, queue_size=1)
self.pub_imu3 = rospy.Publisher('/train/imu3', Imu, queue_size=1)
self.pub_odom = rospy.Publisher('/train/odometry/filtered',
Odometry,
queue_size=1)
self.pub_scan = rospy.Publisher('/train/scan', LaserScan, queue_size=1)
def cb_scan(self, msg):
self.scan = msg
def cb_left_img(self, msg):
self.left_img = msg
def cb_right_img(self, msg):
self.right_img = msg
def cb_depth_img(self, msg):
self.depth_img = msg
def cb_me1_left_img(self, msg):
self.me1_left = msg
def cb_me1_right_img(self, msg):
self.me1_right = msg
def cb_me1_depth_img(self, msg):
self.me1_depth = msg
def cb_me2_left_img(self, msg):
self.me2_left = msg
def cb_me2_right_img(self, msg):
self.me2_right = msg
def cb_me2_depth_img(self, msg):
self.me2_depth = msg
def cb_me3_left_img(self, msg):
self.me3_left = msg
def cb_me3_right_img(self, msg):
self.me3_right = msg
def cb_me3_depth_img(self, msg):
self.me3_depth = msg
def cb_dlm_intention(self, msg):
self.intention = msg.data
def cb_lpe_intention(self, msg):
self.intention = cv2.resize(
undistort(CvBridge().imgmsg_to_cv2(msg, desired_encoding='bgr8')),
(224, 224))
def cb_speed(self, msg):
self.speed = msg.data
def cb_labeled_control(self, msg):
self.labeled_control = msg
def cb_imu(self, msg):
self.imu = msg
def cb_imu1(self, msg):
self.imu1 = msg
def cb_imu2(self, msg):
self.imu2 = msg
def cb_imu3(self, msg):
self.imu3 = msg
def cb_odom(self, msg):
self.odom = msg
def cb_joy(self, data):
self.tele_twist.linear.x = self._scale_x * data.axes[
JOY_MAPPING['axes']['left_stick_ud']]
self.tele_twist.angular.z = self._scale_z * data.axes[
JOY_MAPPING['axes']['left_stick_lr']]
# parse control key
if data.buttons[JOY_MAPPING['buttons']['A']] == 1:
self._enable_auto_control = True
print('Auto control')
if data.buttons[JOY_MAPPING['buttons']['B']] == 1:
self._enable_auto_control = False
print('Manual control')
if data.buttons[JOY_MAPPING['buttons']['back']] == 1:
self.key = 'q'
# toggle recording mode and generate trajectory index if start recording
if data.buttons[JOY_MAPPING['buttons']['start']] == 1:
self.key = 't'
print('toggle training mode to: %s' % (not self.training))
if data.buttons[JOY_MAPPING['buttons']['mode']] == 1:
self.key = 'i'
print('toggle intention mode to: %s' %
(not self.is_manual_intention))
#STRAIGHT_FORWARD
if data.buttons[JOY_MAPPING['buttons']['X']] == 1:
self.manual_intention = 'forward'
print('Intention is manually set to: forward')
#LEFT_TURN
if data.buttons[JOY_MAPPING['buttons']['lt']] == 1:
self.manual_intention = 'left'
print('Intention is manually set to: left')
#RIGHT_TURN
if data.buttons[JOY_MAPPING['buttons']['rt']] == 1:
self.manual_intention = 'right'
print('Intention is manually set to: right')
def _random_string(self, n):
chars = string.ascii_letters + string.digits
ret = ''.join(random.choice(chars) for _ in range(n))
return ret
def _on_loop(self, policy):
"""
Logical loop
"""
self._timer.tick()
if self.key == 'q':
#sys.exit(-1)
self.intention = 'stop'
self.key = ''
if self.key == 't':
self.training = not self.training
self.key = ''
if self.key == 'i':
self.is_manual_intention = not self.is_manual_intention
self.key = ''
if self._enable_auto_control:
if self.is_manual_intention:
intention = Dataset.INTENTION_MAPPING[self.manual_intention]
else:
intention = Dataset.INTENTION_MAPPING[
self.intention] # map intention str => int
if not intention:
print('estimate pose + goal....')
elif intention == 'stop':
self.tele_twist.linear.x = 0
self.tele_twist.angular.z = 0
else:
print('intention: ', intention)
# convert ros msg -> cv2
inp = self.get_data(intention)
pred_control = np.array(policy(*inp))[0]
self.tele_twist.linear.x = pred_control[
0] * Dataset.SCALE_VEL * 0.8
self.tele_twist.angular.z = pred_control[
1] * Dataset.SCALE_STEER * 0.8
# publish to /train/* topic to record data (if in training mode)
if self.training:
self._publish_as_trn()
# publish control
self.control_pub.publish(self.tele_twist)
def get_data(self, intention):
mrgb = cv2.resize(
undistort(
self.bridge.compressed_imgmsg_to_cv2(self.right_img,
desired_encoding='bgr8'),
FRONT_CAMERA_INFO), (3, 224, 224))
mbnw = rgb2gray(mrgb)
rbnw = cv2.resize(
undistort(
self.bridge.compressed_imgmsg_to_cv2(self.me2_left,
desired_encoding='bgr8'),
RIGHT_CAMERA_INFO), (224, 224))
lbnw = cv2.resize(
undistort(
self.bridge.compressed_imgmsg_to_cv2(self.me3_right,
desired_encoding='bgr8'),
LEFT_CAMERA_INFO), (224, 224))
dm = cv2.resize(
self.bridge.compressed_imgmsg_to_cv2(self.depth_img,
desired_encoding='bgr8'),
(224, 224))
dr = cv2.resize(
self.bridge.compressed_imgmsg_to_cv2(self.me2_depth,
desired_encoding='bgr8'),
(224, 224))
dl = cv2.resize(
self.bridge.compressed_imgmsg_to_cv2(self.me3_depth,
desired_encoding='bgr8'),
(224, 224))
#preprocess
mbnw = torch.tensor(mbnw).expand(1, 1, 224, 224).float() / 255.0
rbnw = torch.tensor(rbnw).expand(1, 1, 224, 224).float() / 255.0
lbnw = torch.tensor(lbnw).expand(1, 1, 224, 224).float() / 255.0
dm = torch.tensor(dm).expand(1, 1, 224, 224).float() / 255.0
dr = torch.tensor(dr).expand(1, 1, 224, 224).float() / 255.0
dl = torch.tensor(dl).expand(1, 1, 224, 224).float() / 255.0
intention = torch.tensor([intention]).long()
return [intention, dl, dm, dr, lbnw, mbnw, rbnw]
def _publish_as_trn(self):
if self.odom:
self.pub_trajectory_index.publish(self.trajectory_index)
self.pub_left_img.publish(self.left_img)
self.pub_right_img.publish(self.right_img)
self.pub_depth_img.publish(self.depth_img)
self.pub_me1_left_img.publish(self.me1_left)
self.pub_me1_right_img.publish(self.me1_right)
self.pub_me1_depth_img.publish(self.me1_depth)
self.pub_me2_left_img.publish(self.me2_left)
self.pub_me2_right_img.publish(self.me2_right)
self.pub_me2_depth_img.publish(self.me2_depth)
self.pub_me3_left_img.publish(self.me3_left)
self.pub_me3_right_img.publish(self.me3_right)
self.pub_me3_depth_img.publish(self.me3_depth)
self.pub_teleop_vel.publish(self.tele_twist)
self.pub_intention.publish(self.intention)
self.pub_imu.publish(self.imu)
self.pub_imu1.publish(self.imu1)
self.pub_imu2.publish(self.imu2)
self.pub_imu3.publish(self.imu3)
self.pub_odom.publish(self.odom)
self.pub_scan.publish(self.scan)
def execute(self, policy):
while True:
self._on_loop(policy)
self._rate.sleep()
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
from std_msgs.msg import String
g_is_dead_bot = True
g_red_light_on = False
g_twist = Twist() # default to 0
def dead_bot_switch(msg):
global g_is_dead_bot
if msg.buttons[2] == 1:
g_is_dead_bot = (g_is_dead_bot == False)
# print(g_is_dead_bot)
def got_twist(msg):
global g_twist
g_twist = msg
def safety_switch(msg):
global g_red_light_on
# print(msg.data=="Go")
if msg.data == "Go":
