def name():
'''Create a ROS node and instantiate the class.'''
rospy.init_node('whill_driver', anonymous=True)
port = rospy.get_param('port', '/dev/ttyUSB0')
TREAD = rospy.get_param('tread', 0.5)
rospy.Subscriber("/cmd_vel", Twist, cmd_callback)
x = y = th = 0.0
motor_speed_old = Vector3()
motor_speed_old.x = motor_speed_old.y = 0.0
odom_pub = rospy.Publisher('/odom', Odometry, queue_size=1)
joystick_pub = rospy.Publisher('/joystick', Vector3, queue_size=1)
motor_pub = rospy.Publisher('/motor_speed', Vector3, queue_size=1)
odom_broadcaster = tf.TransformBroadcaster()
odom = Odometry()
whill = WHILL('/dev/ttyUSB0')
whill.StartSendingData(1, 100, 1)
current_time = rospy.Time.now()
last_time = rospy.Time.now()
write_to_port_last_time = rospy.Time.now()
rate = rospy.Rate(100)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
dt = (current_time - last_time).to_sec()
joy_front, joy_side, battery_power, right_motor_speed, left_motor_speed, power_on, error = whill.GetData(
)
#joy_front = joy_side= battery_power= right_motor_speed= left_motor_speed= power_on= error = 0
#modify motor speed
if abs(right_motor_speed - motor_speed_old.x) > 0.5:
right_motor_speed = motor_speed_old.x
motor_speed_old.x = right_motor_speed
if abs(left_motor_speed - motor_speed_old.y) > 0.5:
left_motor_speed = motor_speed_old.y
motor_speed_old.y = left_motor_speed
if joy_front != 0 or joy_side != 0:
WHILL_JOYSTICK_ALLOWED = True
else:
WHILL_JOYSTICK_ALLOWED = False
# compute odometry in a typical way given the velocities of the robot
cv, cw, x, y, th = compute_odometry(x, y, th, right_motor_speed,
left_motor_speed, dt)
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
joystick_msg = Vector3()
joystick_msg.x = joy_front
joystick_msg.y = joy_side
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time,
"base_link", "odom")
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(cv, 0, 0), Vector3(0, 0, cw))
# set the motor speed
motor_speed = Vector3()
motor_speed.x = right_motor_speed
motor_speed.y = left_motor_speed
odom_pub.publish(odom)
joystick_pub.publish(joystick_msg)
motor_pub.publish(motor_speed)
#Only writing to port every 20msec
write_to_port_time = (current_time - write_to_port_last_time).to_sec()
if write_to_port_time >= 0.02:
# dv:velocity is in m/sec [-0.5:1.6]; dw:angular velocity rad/sec [-1:1]; joystick commands [100:-100]
jv, jw = set_velocity_to_joystick_range(dv, dw)
if WHILL_JOYSTICK_ALLOWED == True:
whill.SetJoystick(1, 0, 0)
else:
whill.SetJoystick(0, jv, jw)
write_to_port_last_time = current_time
#whill.SetJoystick(0, int(dv*100), int(dw*-100))
rospy.sleep(0.001)
last_time = current_time
rate.sleep()
def get_ros_quaternion(roll, pitch, yaw):
return Quaternion(*quaternion_from_euler(roll, pitch,
yaw)) # returns Quaternion
def __init__(self):
rospy.init_node('MultiGoalListen', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(120))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting MultiGoalListen")
rospy.Subscriber('/robot_pose', Pose, self.robot_pose_callback)
self.r = redis.Redis(host="127.0.0.1", port=6379, db=0)
attr_dict = {
"mode": "order",
"loopWay": "auto",
"isNavNext": 0,
"currentState": "stopped",
"goalQueue": "GoalQueueA",
"priorGoalQueue": "GoalQueueB",
"currentQueue": "",
"currentGoal": "",
"successNum": "0",
"failedNum": "0",
"intervalTime": "3",
}
self.r.hmset("GoalState", attr_dict)
goal = MoveBaseGoal()
quaternion = Quaternion()
while not rospy.is_shutdown():
#goal 1
mymode = self.r.hget("GoalState", "mode")
priorgoalQueue = self.r.hget("GoalState", "priorGoalQueue")
goalQueue = self.r.hget("GoalState", "goalQueue")
loopWay = self.r.hget("GoalState", "loopWay")
isNavNext = self.r.hget("GoalState", "isNavNext")
if priorgoalQueue != None and self.r.llen(priorgoalQueue) > 0:
mygoalQueue = priorgoalQueue
else:
mygoalQueue = goalQueue
mycurrentState = self.r.hget("GoalState", "currentState")
if mycurrentState != "running":
rospy.sleep(1)
continue
if loopWay == "manual":
if int(isNavNext) == 1:
self.r.hset("GoalState", "isNavNext", 0)
else:
rospy.sleep(1)
continue
self.r.hset("GoalState", "currentQueue", mygoalQueue)
mygoal = self.r.lpop(mygoalQueue)
if mygoalQueue == goalQueue and mymode == "loop":
self.r.rpush(mygoalQueue, mygoal)
rospy.loginfo("rpush:" + mygoalQueue + " " + mygoal)
if mygoal != None:
self.r.hset("GoalState", "currentGoal", mygoal)
mygoalVal = self.r.hget("GoalAliaseSet", mygoal)
if mygoalVal != None:
x, y, z, qx, qy, qz, qw = str.split(mygoalVal, '_')
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
quaternion.x = float(qx)
quaternion.y = float(qy)
quaternion.z = float(qz)
quaternion.w = float(qw)
goal.target_pose.pose.position.x = float(x)
goal.target_pose.pose.position.y = float(y)
goal.target_pose.pose.position.z = float(z)
goal.target_pose.pose.orientation = quaternion
self.move_base.send_goal(goal)
# Allow 1 minute to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(600))
# If we don't get there in time, abort the goal
if not finished_within_time:
self.move_base.cancel_goal()
rospy.logwarn("Timed out achieving goal")
self.r.hincrby("GoalState", "failedNum")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
self.r.hincrby("GoalState", "successNum")
else:
rospy.logwarn("Goal failed, state:" + str(state))
self.r.hincrby("GoalState", "failedNum")
else:
rospy.loginfo("all Goales have been finished")
self.r.hset("GoalState", "currentState", "stopped")
rospy.sleep((int)(self.r.hget("GoalState", "intervalTime")))
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
try:
left_pidin, right_pidin = self.arduino.get_pidin()
except:
rospy.logerr("getpidout exception count: ")
return
self.lEncoderPub.publish(left_pidin)
self.rEncoderPub.publish(right_pidin)
try:
left_pidout, right_pidout = self.arduino.get_pidout()
except:
rospy.logerr("getpidout exception count: ")
return
self.lPidoutPub.publish(left_pidout)
self.rPidoutPub.publish(right_pidout)
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
#rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
if (left_enc < self.encoder_low_wrap
and self.enc_left > self.encoder_high_wrap):
self.l_wheel_mult = self.l_wheel_mult + 1
elif (left_enc > self.encoder_high_wrap
and self.enc_left < self.encoder_low_wrap):
self.l_wheel_mult = self.l_wheel_mult - 1
else:
self.l_wheel_mult = 0
if (right_enc < self.encoder_low_wrap
and self.enc_right > self.encoder_high_wrap):
self.r_wheel_mult = self.r_wheel_mult + 1
elif (right_enc > self.encoder_high_wrap
and self.enc_right < self.encoder_low_wrap):
self.r_wheel_mult = self.r_wheel_mult - 1
else:
self.r_wheel_mult = 0
#dright = (right_enc - self.enc_right) / self.ticks_per_meter
#dleft = (left_enc - self.enc_left) / self.ticks_per_meter
dleft = 1.0 * (left_enc + self.l_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_left) / self.ticks_per_meter
dright = 1.0 * (right_enc + self.r_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_right) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def handle_traj_gen(req):
vel_constr = 2
angle_constr = 3.14159265 / 4
yaw_init_state = 0
yaw_locked = True
if yaw_locked:
yaw_constr_plus = yaw_init_state
yaw_constr_minus = yaw_init_state
else:
yaw_constr_plus = 3.14159265 * 4
yaw_constr_minus = -3.14159265 * 4
input_constr = 6
x0pos = [0, 0, 1]
x0vel = [0, 0, 0]
x0ang = [0, 0, yaw_init_state]
x0angrate = [0, 0, 0]
x1pos = [2., 0., 2.5]
x1vel_y = 0
x1vel_z = 0
x2pos = [2., 4., 2.5]
#??? x y z mistake
x2vel_y = 0
x2vel_z = 0
xFpos = [0, 0, 1.2]
xFvel = [0, 0, 0]
xFang = [0, 0, yaw_init_state]
xFangrate = [0, 0, 0]
N = 25
# x = vertcat(pos,vel,ang,angrate)
x = MX.sym('x', 12)
u = MX.sym('u', 4)
t_transf = MX.sym('t_transf')
t = MX.sym('t')
xdot = diff_eq_time_trans(0, x, u, t_transf)
L = (u[0]**2 + u[1]**2 + u[2]**2 + u[3]**2) * t_transf
f = Function('f', [t, x, u, t_transf], [xdot])
fq = Function('fq', [t, x, u, t_transf], [L])
X0 = MX.sym('X0', 12)
U = MX.sym('U', 4)
TF = MX.sym('TF')
M = 1
DT = 1. / N / M
X = X0
Q = 0
f_handle = lambda t, x, u: f(t, x, u, TF)
fq_handle = lambda t, x, u: fq(t, x, u, TF)
for j in range(M):
X = rk4(f_handle, DT, 0., X, U)
Q = rk4(fq_handle, DT, 0., 0, U)
F = Function('F', [X0, U, TF], [X, Q], ['x0', 'p', 'tf_interval'],
['xf', 'qf'])
# Start with an empty NLP
w = []
w0 = []
lbw = []
ubw = []
J = 0
g = []
lbg = []
ubg = []
# "Lift" initial conditions
#
t_interval_1 = MX.sym('t_interval_1')
t_interval_2 = MX.sym('t_interval_2')
t_interval_3 = MX.sym('t_interval_3')
#
Xk = MX.sym('X0', 12)
w += [Xk]
# this initial condition has the specified bound,
# with only one possible solution
# x0(0)=1; x1(0)=0;
lbw += x0pos + x0vel + x0ang + x0angrate
ubw += x0pos + x0vel + x0ang + x0angrate
#solver's guess
w0 += x0pos + x0vel + x0ang + x0angrate
gravity, mass, kF, kM, Len = project_parameters()
for k in range(N):
Uk = MX.sym('U_' + str(k), 4)
w += [Uk]
if k != 0:
lbw += [0, 0, 0, 0]
ubw += [input_constr, input_constr, input_constr, input_constr]
else:
lbw += [
0.25 * gravity * mass, 0.25 * gravity * mass,
0.25 * gravity * mass, 0.25 * gravity * mass
]
ubw += [
0.25 * gravity * mass, 0.25 * gravity * mass,
0.25 * gravity * mass, 0.25 * gravity * mass
]
w0 += [
0.25 * gravity * mass, 0.25 * gravity * mass,
0.25 * gravity * mass, 0.25 * gravity * mass
]
Fk = F(x0=Xk, p=Uk, tf_interval=t_interval_1)
Xk_end = Fk['xf']
J = J + Fk['qf']
Xk = MX.sym('X_' + str(k + 1), 12)
w += [Xk]
if k != N - 1:
lbw += [
-inf, -inf, -inf, -vel_constr, -vel_constr, -vel_constr,
-angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf,
-inf
]
ubw += [
inf, inf, inf, vel_constr, vel_constr, vel_constr,
angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf
]
else:
lbw += x1pos + [
-vel_constr, x1vel_y, x1vel_z, -angle_constr, -angle_constr,
yaw_constr_minus, -inf, -inf, -inf
]
ubw += x1pos + [
vel_constr, x1vel_y, x1vel_z, angle_constr, angle_constr,
yaw_constr_plus, inf, inf, inf
]
w0 += x0pos + [0, 0, 0, 0, 0, 3, 0, 0, 0]
g += [Xk_end - Xk]
lbg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
ubg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for k in range(N, 2 * N):
Uk = MX.sym('U_' + str(k), 4)
w += [Uk]
lbw += [0, 0, 0, 0]
ubw += [input_constr, input_constr, input_constr, input_constr]
w0 += [
0.25 * gravity * mass, 0.25 * gravity * mass,
0.25 * gravity * mass, 0.25 * gravity * mass
]
Fk = F(x0=Xk, p=Uk, tf_interval=t_interval_2)
Xk_end = Fk['xf']
J = J + Fk['qf']
Xk = MX.sym('X_' + str(k + 1), 12)
w += [Xk]
if k != 2 * N - 1:
lbw += [
-inf, -inf, -inf, -vel_constr, -vel_constr, -vel_constr,
-angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf,
-inf
]
ubw += [
inf, inf, inf, vel_constr, vel_constr, vel_constr,
angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf
]
else:
#??? x y z mistake
lbw += x2pos + [
-vel_constr, x2vel_y, x2vel_z, -angle_constr, -angle_constr,
yaw_constr_minus, -inf, -inf, -inf
]
ubw += x2pos + [
vel_constr, x2vel_y, x2vel_z, angle_constr, angle_constr,
yaw_constr_plus, inf, inf, inf
]
w0 += x1pos + [0, 0, 0, 0, 0, 3, 0, 0, 0]
g += [Xk_end - Xk]
lbg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
ubg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for k in range(2 * N, 3 * N):
Uk = MX.sym('U_' + str(k), 4)
w += [Uk]
if k != 3 * N - 1:
lbw += [0, 0, 0, 0]
ubw += [input_constr, input_constr, input_constr, input_constr]
else:
lbw += [
0.25 * gravity * mass, 0.25 * gravity * mass,
0.25 * gravity * mass, 0.25 * gravity * mass
]
ubw += [
0.25 * gravity * mass, 0.25 * gravity * mass,
0.25 * gravity * mass, 0.25 * gravity * mass
]
w0 += [
0.25 * gravity * mass, 0.25 * gravity * mass,
0.25 * gravity * mass, 0.25 * gravity * mass
]
Fk = F(x0=Xk, p=Uk, tf_interval=t_interval_3)
Xk_end = Fk['xf']
J = J + Fk['qf']
Xk = MX.sym('X_' + str(k + 1), 12)
w += [Xk]
if k != 3 * N - 1:
lbw += [
-inf, -inf, -inf, -vel_constr, -vel_constr, -vel_constr,
-angle_constr, -angle_constr, yaw_constr_minus, -inf, -inf,
-inf
]
ubw += [
inf, inf, inf, vel_constr, vel_constr, vel_constr,
angle_constr, angle_constr, yaw_constr_plus, inf, inf, inf
]
else:
lbw += xFpos + xFvel + xFang + xFangrate
ubw += xFpos + xFvel + xFang + xFangrate
w0 += x2pos + [0, 0, 0, 0, 0, 3, 0, 0, 0]
g += [Xk_end - Xk]
lbg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
ubg += [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
w += [t_interval_1, t_interval_2, t_interval_3]
lbw += [0.1, 0.1, 0.1]
ubw += [inf, inf, inf]
w0 += [2, 3, 2]
# g += [t_interval_1,t_interval_2,t_interval_3]
# lbg += [0,0,0]
# ubg += [inf,inf,inf]
prob = {'f': J, 'x': vertcat(*w), 'g': vertcat(*g)}
solver = nlpsol('solver', 'ipopt', prob)
sol = solver(x0=w0, lbx=lbw, ubx=ubw, lbg=lbg, ubg=ubg)
w_opt_all = sol['x'].full().flatten()
t1_opt = w_opt_all[-3]
t2_opt = w_opt_all[-2]
t3_opt = w_opt_all[-1]
print(t1_opt)
print(t2_opt)
print(t3_opt)
w_opt = np.delete(w_opt_all, [-3, -2, -1])
pos_x_opt = w_opt[0::16]
pos_y_opt = w_opt[1::16]
pos_z_opt = w_opt[2::16]
vel_x_opt = w_opt[3::16]
vel_y_opt = w_opt[4::16]
vel_z_opt = w_opt[5::16]
ang_x_opt = w_opt[6::16]
ang_y_opt = w_opt[7::16]
ang_z_opt = w_opt[8::16]
angr_x_opt = w_opt[9::16]
angr_y_opt = w_opt[10::16]
angr_z_opt = w_opt[11::16]
u1_opt = []
u2_opt = []
u3_opt = []
u4_opt = []
for k in range(3 * N):
u1_opt += [w_opt[12 + k * 16]]
u2_opt += [w_opt[13 + k * 16]]
u3_opt += [w_opt[14 + k * 16]]
u4_opt += [w_opt[15 + k * 16]]
tgrid1 = [t1_opt / N * k for k in range(N)]
tgrid2 = [t1_opt + t2_opt / N * k for k in range(N)]
tgrid3 = [t1_opt + t2_opt + t3_opt / N * k for k in range(N + 1)]
tgrid3u = [t1_opt + t2_opt + t3_opt / N * k for k in range(N)]
tgrid = tgrid1 + tgrid2 + tgrid3
tgridu = tgrid1 + tgrid2 + tgrid3u
msg = MultiDOFJointTrajectory()
msg.header.frame_id = ''
msg.header.stamp = rospy.Time.now()
msg.joint_names = ["base_link"]
for i in range(len(tgrid)):
transforms = Transform()
transforms.translation.x = pos_x_opt[i]
transforms.translation.y = pos_y_opt[i]
transforms.translation.z = pos_z_opt[i]
quaternion = tf.transformations.quaternion_from_euler(
ang_x_opt[i], ang_y_opt[i], ang_z_opt[i])
transforms.rotation = Quaternion(quaternion[0], quaternion[1],
quaternion[2], quaternion[3])
velocities = Twist()
velocities.linear.x = vel_x_opt[i]
velocities.linear.y = vel_y_opt[i]
velocities.linear.z = vel_z_opt[i]
# velocities.angular.x =
accelerations = Twist()
# time_from_start += rospy.Duration.from_sec(waypoints[i].waiting_time)
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations],
rospy.Duration.from_sec(tgrid[i]))
msg.points.append(point)
return msg
def empty_transform():
t = TransformStamped()
t.transform.rotation = Quaternion(0, 0, 0, 1)
t.header.frame_id = "tool_link"
t.child_frame_id = "TCP"
return t
def deliver_sample(move_arm, args):
move_arm.set_planner_id("RRTstar")
x_delivery = args[1]
y_delivery = args[2]
z_delivery = args[3]
# after sample collect
mypi = 3.14159
d2r = mypi / 180
r2d = 180 / mypi
goal_pose = move_arm.get_current_pose().pose
#position was found from rviz tool
goal_pose.position.x = x_delivery
goal_pose.position.y = y_delivery
goal_pose.position.z = z_delivery
r = -179
p = -20
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
plan = move_arm.plan()
if len(plan.joint_trajectory.points) == 0: # If no plan found, abort
return False
plan = move_arm.go(wait=True)
move_arm.stop()
###rotate scoop to deliver sample at current location...
# adding position constraint on the solution so that the tip doesnot diverge to get to the solution.
pos_constraint = PositionConstraint()
pos_constraint.header.frame_id = "base_link"
pos_constraint.link_name = "l_scoop"
pos_constraint.target_point_offset.x = 0.1
pos_constraint.target_point_offset.y = 0.1
pos_constraint.target_point_offset.z = 0.1 ###rotate scoop to deliver sample at current location begin
pos_constraint.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
pos_constraint.weight = 1
#using euler angles for own verification..
r = +180
p = 90 # 45 worked get
y = -90
q = quaternion_from_euler(r * d2r, p * d2r, y * d2r)
goal_pose = move_arm.get_current_pose().pose
rotation = (goal_pose.orientation.x, goal_pose.orientation.y,
goal_pose.orientation.z, goal_pose.orientation.w)
euler_angle = euler_from_quaternion(rotation)
goal_pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
move_arm.set_pose_target(goal_pose)
plan = move_arm.plan()
if len(plan.joint_trajectory.points) == 0: # If no plan found, abort
return False
plan = move_arm.go(wait=True)
move_arm.stop()
move_arm.clear_pose_targets()
move_arm.set_planner_id("RRTconnect")
return True
def RRT(self, starting_joint_angles, pose_angles, obstacle_pose,
obstacle_radius):
overhead_orientation = Quaternion(x=-0.0249590815779,
y=0.999649402929,
z=0.00737916180073,
w=0.00486450832011)
P1 = mat([[
starting_joint_angles.get('left_s0'),
starting_joint_angles.get('left_s1'),
starting_joint_angles.get('left_e0'),
starting_joint_angles.get('left_e1'),
starting_joint_angles.get('left_w0'),
starting_joint_angles.get('left_w1'),
starting_joint_angles.get('left_w2')
]])
Pf = mat([[
pose_angles.get('left_s0'),
pose_angles.get('left_s1'),
pose_angles.get('left_e0'),
pose_angles.get('left_e1'),
pose_angles.get('left_w0'),
pose_angles.get('left_w1'),
pose_angles.get('left_w2')
]])
if self.robotcollision(P1, obstacle_pose,
obstacle_radius) == 1 or self.robotcollision(
Pf, obstacle_pose, obstacle_radius) == 1:
print("starting point or end point in collision")
exit(1)
qMilestones = vstack((P1, Pf))
iteration = 300
qMax = mat([[0.5, -0.5, -0.5, 2.3, 1, 1.5, 0]])
qMin = mat([[-0.5, -1.5, -1.5, 1, 0, 0.5, -1]])
while self.checkedge(qMilestones[-1, :], qMilestones[-2, :],
obstacle_pose, obstacle_radius) == 1:
Pt_set = np.matlib.repmat(qMin, iteration, 1) + multiply(
np.matlib.repmat((qMax - qMin), iteration, 1),
np.random.uniform(0, 1, [iteration, 7]))
useless = []
for i in range(len(Pt_set)):
posei = self.fk_request(Pt_set[i, :], 4)[0:3, 3]
Bool = self.ik_request(
Pose(position=Point(x=posei[0, 0],
y=posei[1, 0] - 0.01,
z=posei[2, 0] - 1.6),
orientation=overhead_orientation))
if Bool == False:
useless.append(i)
Pt_set = delete(Pt_set, useless, axis=0)
Pt_sum = vstack((P1, Pt_set, Pf))
n = len(Pt_sum)
dismat = np.zeros([n, n])
for c in range(n):
pointc = Pt_sum[c]
for d in range(n):
pointd = Pt_sum[d]
D = pointc - pointd
distance = sqrt(np.dot(D, D.T))
dismat[c, d] = distance
dismat[d, c] = distance
dismat2 = np.zeros([n, n])
for i in range(1, n - 1):
qnear = np.argmin(dismat[0:i, i])
print(qnear)
while self.checkedge(Pt_sum[i, :], Pt_sum[qnear, :],
obstacle_pose, obstacle_radius) == 1:
D = Pt_sum[i, :] - Pt_sum[qnear, :]
Pt_sum[i, :] = Pt_sum[qnear, :] + 0.5 * D
for a in range(n - 1):
pointa = Pt_sum[a]
for b in range(n - 1):
pointb = Pt_sum[b]
D = pointa - pointb
distance = sqrt(np.dot(D, D.T))
dismat[a, b] = distance
dismat[b, a] = distance
dismat2[i, qnear] = 1
dismat2[qnear, i] = 1
qnear = np.argmin(dismat[0:n - 1, n - 1])
dismat2[n - 1, qnear] = 1
dismat2[qnear, n - 1] = 1
qMilestones = Pt_sum[-1, :]
j = len(Pt_sum) - 1
while j > 0:
nodepos = argmax(dismat[0:j + 1, j])
qMilestones = vstack((Pt_sum[nodepos], qMilestones))
j = nodepos
iteration = iteration + 50
return qMilestones
def main():
"""RSDK Inverse Kinematics Pick and Place Example
A Pick and Place example using the Rethink Inverse Kinematics
Service which returns the joint angles a requested Cartesian Pose.
This ROS Service client is used to request both pick and place
poses in the /base frame of the robot.
Note: This is a highly scripted and tuned demo. The object location
is "known" and movement is done completely open loop. It is expected
behavior that Baxter will eventually mis-pick or drop the block. You
can improve on this demo by adding perception and feedback to close
the loop.
"""
rospy.init_node("ik_pick_and_place_demo")
# Load Gazebo Models via Spawning Services
# Note that the models reference is the /world frame
# and the IK operates with respect to the /base frame
load_gazebo_models()
# Remove models from the scene on shutdown
rospy.on_shutdown(delete_gazebo_models)
# Wait for the All Clear from emulator startup
rospy.wait_for_message("/robot/sim/started", Empty)
limb = 'left'
hover_distance = 0.15 # meters
# Starting Joint angles for left arm
starting_joint_angles = {
'left_w0': 0.6699952259595108,
'left_w1': 1.030009435085784,
'left_w2': -0.4999997247485215,
'left_e0': -1.189968899785275,
'left_e1': 1.9400238130755056,
'left_s0': -0.08000397926829805,
'left_s1': -0.9999781166910306
}
overhead_orientation = Quaternion(x=-0.0249590815779,
y=0.999649402929,
z=0.00737916180073,
w=0.00486450832011)
pnp = PickAndPlace(limb, hover_distance)
# An orientation for gripper fingers to be overhead and parallel to the obj
ball_poses = list()
container_pose = Pose(position=Point(x=0.5275, y=0.6675, z=-0.14),
orientation=overhead_orientation)
# The Pose of the block in its initial location.
# You may wish to replace these poses with estimates
# from a perception node.
ball_poses.append(
Pose(position=Point(x=0.7, y=0.15, z=-0.14),
orientation=overhead_orientation))
ball_poses.append(
Pose(position=Point(x=0.75, y=0, z=-0.14),
orientation=overhead_orientation))
ball_poses.append(
Pose(position=Point(x=0.65, y=0.3, z=-0.14),
orientation=overhead_orientation))
# Feel free to add additional desired poses for the object.
# Each additional pose will get its own pick and place.
# Move to the desired starting angles
pnp._guarded_move_to_joint_position(starting_joint_angles)
for i in range(len(ball_poses)):
print("\nPicking...")
pnp.pick(ball_poses[i])
print("\nPlacing...")
pnp.place(container_pose)
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 2)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
locations = dict()
#定义四个地点的位置
locations['起始点'] = Pose(Point(1.309, 0.919, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
locations['客厅'] = Pose(Point(3.199, 0.513, 0.000),
Quaternion(00.000, 0.000, 0.000, 1.000))
locations['厨房'] = Pose(Point(5.581, 0.977, 0.000),
Quaternion(0.000, 0.000, 0.752, 0.660))
locations['卧室'] = Pose(Point(6.223, -2.749, 0.000),
Quaternion(0.000, 0.000, 0.985, -0.170))
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_loop = 0 #定义控制循环，当为1时停止循环
first = 0 #定义第一次变量
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
self.Dist = 0
self.Asr = ""
Loc_1 = 0
Loc_2 = 0
Loc_3 = 0
Name_1 = 0
name_2 = 0
Name = ""
Drink_1 = 0
Drink_2 = 0
Drink_3 = 0
Drink = ""
# Get the initial pose from the user (how to set)
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
#dict_keys = ['起始点','厨房','卧室']
dict_keys = ['客厅', '卧室', '厨房']
pub1 = rospy.Publisher('/voice/xf_tts_topic', String,
queue_size=5) #tts
pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32,
queue_size=5) #asr
#pub3 = rospy.Publisher('arm',String,queue_size=5) #arm
#pub4 = rospy.Publisher('tracking',Int32,queue_size=5) #物体识别
pub5 = rospy.Publisher('/following', Int32, queue_size=5) #人体跟随
#pub6 = rospy.Publisher('drink_type',String,queue_size=5) #拿的物体类别
pub7 = rospy.Publisher('ros2_wake', Int32, queue_size=5) #ros2opencv2
#pub8 = rospy.Publisher('/voice/xf_nav_follow',String,queue_size=5) #nav_follow
#######################################
# Begin the main loop and run through a sequence of locations
while n_loop != 1:
if first == 0: #如果是第一次
i = 0
location = "起始点"
first += 1
# sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
#if dict_keys[0] == last_location:
# i = 1
#####################################
####### Modify by XF 2017.4.14 ######
# location = sequence[i]
# rospy.loginfo("location= " + str(location))
# location = locations.keys()[i]
#location = dict_keys[i]
#####################################
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
#last_location = location
#rospy.loginfo("test 1 last_location="+str(last_location))
# Increment the counters
#i += 1
#rospy.loginfo("test 2 i="+str(i))
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 6 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(360))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.ABORTED: # can not find a plan,give feedback
rospy.loginfo("please get out")
status_n = "please get out"
pub1.publish(status_n)
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
############ add voice by XF 4.25 ################## The 1st
# pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5)
loc = "主人，我已到" + str(location)
pub1.publish(loc) #tts
rospy.sleep(5)
# rospy.Subscriber('dist',Int32,self.callbackDist) #now robot already adjust correctly
# rospy.loginfo("Dist:"+str(self.Dist))
commandA1 = "您好，请问有什么指示?"
pub1.publish(commandA1)
rospy.sleep(3)
#while self.Asr == "":
#awake=1
#pub2.publish(awake) #asr
#rospy.Subscriber('/voice/xf_asr_follow',String,self.callbackAsr) #获取asr发过来的说话内容
#rospy.sleep(10)
#rospy.loginfo("self.Asr:"+str(self.Asr))
#if self.Asr != "":
# self.Asr=""
#while self.Asr == "":
awake = 1
pub2.publish(awake) #asr
rospy.Subscriber('/voice/xf_asr_follow', String,
self.callbackAsr) #获取asr发过来的说话内容
rospy.sleep(10)
rospy.loginfo("self.Asr:" + str(self.Asr))
#if self.Asr != "":
# self.Asr=""
#rospy.sleep(10)
Loc_1 = string.find(
self.Asr, '客厅') #find函数，返回所查字符串在整个字符串的起始位置，如果没有，则返回-1
Loc_2 = string.find(self.Asr, '厨房')
Loc_3 = string.find(self.Asr, '卧室')
#Name_1=string.find(self.Asr,'郭晓萍')
#Name_2=string.find(self.Asr,'方璐')
# Drink_1=string.find(self.Asr,'可乐')
# Drink_2=string.find(self.Asr,'雪碧')
# Drink_3=string.find(self.Asr,'橙汁')
if Loc_1 != -1:
rospy.loginfo("匹配客厅")
#dict_keys = [last_location,'客厅']
last_location = location
location = "客厅"
if Loc_2 != -1:
rospy.loginfo("匹配厨房")
last_location = location
location = "厨房"
#dict_keys = [last_location,'卧室']
if Loc_3 != -1:
rospy.loginfo("匹配卧室")
last_location = location
#dict_keys=[last_location,'厨房']
location = "卧室"
#if Name_1!=-1:
# Name = '郭晓萍'
# rospy.loginfo("匹配郭晓萍")
#if Name_2!=-1:
# Name = '方璐'
# rospy.loginfo("匹配方璐")
# if Drink_1!=-1:
# Drink = '可乐'
# if Drink_2!=-1:
# Drink = '雪碧'
# if Drink_3!=-1:
# Drink = '橙汁'
# ros2_wake=1
# pub7.publish(ros2_wake)
# pub6.publish(Drink)
#######################################
self.Asr = ""
rospy.sleep(5)
commandA2 = "收到指示，请稍等."
pub1.publish(commandA2)
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " + str(100 * n_successes / n_goals) +
"%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) +
" m")
rospy.sleep(self.rest_time)
def __init__(self):
self.SVL = 0
self.SVR = 0
rospy.init_node('odometry_publisher')
rospy.Subscriber('/ard_odom', Twist, self.ard_odom_callback)
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
x = 0.0
y = 0.0
th = 0.0
v = 0
w = 0
wheelSeparation = 0.6
#values from 0.95 - 1.05
odomTurnMultiplier = 0.95
current_time = rospy.Time.now()
last_time = rospy.Time.now()
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
# compute odometry in a typical way given the velocities of the robot
dt = (current_time - last_time).to_sec()
DL = dt * self.SVL
DR = dt * self.SVR
dxy = (DL + DR) / 2
dth = (((DR - DL) / wheelSeparation)) * odomTurnMultiplier
x += dxy * cos(th)
y += dxy * sin(th)
th += dth
v = dxy / dt
w = dth / dt
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time,
"base_link", "odom")
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(v, 0, 0), Vector3(0, 0, w))
# publish the message
odom_pub.publish(odom)
last_time = current_time
r.sleep()
import rospy
from gazebo_msgs.srv import SpawnModel, DeleteModel
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from std_msgs.msg import Empty
from tf.transformations import quaternion_from_euler as e2q
pi = 3.14159265
model_path = '/home/andrey/.gazebo/models/'
table_path = model_path + 'table/model.sdf'
table_pose = Pose(position=Point(x=1.0, y=0.0, z=0.0),
orientation=Quaternion(*e2q(0.0, 0.0, -pi/2)))
drum1_path = model_path + 'disk_part/static_model.sdf'
drum1_pose = Pose(position=Point(x=0.79, y=0.42, z=1.02))
drum2_path = model_path + 'cinder_block_2/static_model.sdf'
drum2_pose = Pose(position=Point(x=0.89, y=0.00, z=1.02))
drum3_path = model_path + 'pulley_part/static_model.sdf'
drum3_pose = Pose(position=Point(x=0.76, y=-0.40, z=1.02))
def spawn_models(*model_paths_and_poses):
"""takes series of (path,pose) pairs and spawns them"""
rospy.wait_for_service('/gazebo/spawn_sdf_model')
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
for tup in model_paths_and_poses:
sys.exit(0)
pub = rospy.Publisher('imu', Imu, queue_size=1)
diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
diag_pub_time = rospy.get_time();
imuMsg = Imu()
imuMsg.header.frame_id = 'map'
diag_arr = DiagnosticArray()
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
diag_msg.message = 'Received AHRS measurement'
marker = Marker(color=ColorRGBA(r=1, g=1, b=1, a=1), type=9, id=0, scale=Vector3(x=0, y=0, z=0.14), pose=Pose(position=Vector3(x=0.5, y=0.5, z=1.45), orientation=Quaternion(w=1, x=0, y=0, z=0)), ns="imu")
roll=0
pitch=0
yaw=0
seq=0
degrees2rad = math.pi/180.0
rospy.loginfo("Giving the razor IMU board 3 seconds to boot...")
rospy.sleep(3) # Sleep for 5 seconds to wait for the board to boot
rospy.loginfo("Flushing first 30 IMU entries...")
for x in range(0, 30):
line = ser.readline()
def update_odom(self):
t2 = rospy.Time.now()
t1 = self.prev_time
self.prev_time = t2
dt = (t2 - t1).to_sec()
accel = (self.v_cmd - self.v) / dt
mu_v = np.sign(accel)
if (abs(accel) > self.max_accel):
accel = self.max_accel * mu_v
dmeters = self.v * dt + 0.5 * accel * dt**2
self.v += accel * dt
w_dot = (self.w_cmd - self.w) / dt
mu_w = np.sign(w_dot)
if (abs(w_dot) > self.max_w_dot):
w_dot = self.max_w_dot * mu_w
dtheta = self.w * dt + 0.5 * w_dot * dt**2
self.w += w_dot * dt
#update bot position
self.bot_rad = self.bot_rad + dtheta
dx = dmeters * np.cos(self.bot_rad)
dy = dmeters * np.sin(self.bot_rad)
self.botx = self.botx + dx
self.boty = self.boty + dy
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.bot_rad)
self.odom_broadcaster.sendTransform((self.botx, self.boty, 0.),
odom_quat, t2, "base_link", "odom")
odom = Odometry()
odom.header.stamp = t2
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(self.botx, self.boty, 0.),
Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(self.v, 0, 0), Vector3(0, 0, self.w))
# publish the message
self.odom_pub.publish(odom)
# LaserScan simulation
# initially just check point obstacle at 10,0 in odom
if (self.USE_SIMPLE_SCAN_SIM):
scan = LaserScan()
scan.header.frame_id = 'laser'
scan.range_min = 0.2
scan.range_max = 30.0
fov = 48
nDet = 16
current_time = rospy.Time.now()
scan.header.stamp = current_time
scan.angle_min = -fov / 2 * 3.14 / 180.0
scan.angle_max = fov / 2 * 3.14 / 180.0
scan.angle_increment = (fov / nDet) * 3.14 / 180.0
scan.time_increment = 0.001
scan.ranges = []
obstacle_found = False
dx = 10.0 - self.botx
dy = 0.0 - self.boty
theta = math.atan2(dy, dx)
if (abs(self.bot_rad - theta) < fov / 2 * 3.14 / 180.0):
dist = math.sqrt(dx**2 + dy**2)
if (dist < 30.0):
obstacle_found = True
for sub_theta_deg in range(-fov / 2, fov / 2 + 1,
fov / nDet):
if (abs(self.bot_rad + sub_theta_deg * 3.14 / 180.0 -
theta) < scan.angle_increment):
scan.ranges.append(dist)
else:
scan.ranges.append(50.0)
if not obstacle_found:
for k in range(nDet):
scan.ranges.append(50.0)
self.scan_pub.publish(scan)
confMode = False
print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}\tSys_cal={3} Gyro_cal={4} Accel_cal={5} Mag_cal={6}'.format(
heading, roll, pitch, sys, gyro, accel, mag))
#imuData = { "heading":heading, "roll":roll, "pitch":pitch, "sys":sys, "gyro":gyro, "accel":accel, "mag":mag }
i.header.stamp = rospy.Time.now()
ps.header = i.header
axes_.yaw = heading
axes_.pitch = pitch
axes_.roll = roll
axes_pub.publish(axes_)
# Other values you can optionally read:
# Orientation as a quaternion:
q = Quaternion()
q.x, q.y, q.z, q.w = bno.read_quaternion()
i.orientation = q
v = Vector3()
v.x, v.y, v.z = roll, pitch, heading
i.angular_velocity = v
# Sensor temperature in degrees Celsius:
#temp_c = bno.read_temp()
# Magnetometer data (in micro-Teslas):
#x,y,z = bno.read_magnetometer()
# Gyroscope data (in degrees per second):
#x, y, z = bno.read_gyroscope()
# Accelerometer data (in meters per second squared):
#x,y,z = bno.read_accelerometer()
# Linear acceleration data (i.e. acceleration from movement, not gravity--
# returned in meters per second squared):
def __init__(self):
rospy.init_node('random_navigation', anonymous=True)
rospy.on_shutdown(self.shutdown)
# 在每个目标位置暂停的时间
self.rest_time = rospy.get_param("~rest_time", 2)
# 到达目标的状态
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# 设置目标点的位置
# 如果想要获得某一点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中一点
# 在终端中就会看到坐标信息
locations = dict()
locations['p1'] = Pose(Point(1.150, 5.461, 0.000),
Quaternion(0.000, 0.000, -0.013, 1.000))
locations['p2'] = Pose(Point(6.388, 2.66, 0.000),
Quaternion(0.000, 0.000, 0.063, 0.998))
locations['p3'] = Pose(Point(8.089, -1.657, 0.000),
Quaternion(0.000, 0.000, 0.946, -0.324))
locations['p4'] = Pose(Point(9.767, 5.171, 0.000),
Quaternion(0.000, 0.000, 0.139, 0.990))
locations['p5'] = Pose(Point(0.502, 1.270, 0.000),
Quaternion(0.000, 0.000, 0.919, -0.392))
locations['p6'] = Pose(Point(4.557, 1.234, 0.000),
Quaternion(0.000, 0.000, 0.627, 0.779))
# 发布控制机器人的消息
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 订阅move_base服务器的消息
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# 60s等待时间限制
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# 保存机器人的在rviz中的初始位置
initial_pose = PoseWithCovarianceStamped()
# 保存成功率、运行时间、和距离的变量
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# 确保有初始位置
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# 开始主循环，随机导航
if not rospy.is_shutdown():
# 在当前的排序中获取下一个目标点
location = 'p1'
# 跟踪行驶距离
# 使用更新的初始位置
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# 存储上一次的位置，计算距离
last_location = location
# 计数器加1
i += 1
n_goals += 1
# 设定下一个目标点
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# 让用户知道下一个位置
rospy.loginfo("Going to: " + str(location))
# 向下一个位置进发
self.move_base.send_goal(self.goal)
# 五分钟时间限制
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# 查看是否成功到达
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# 运行所用时间
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# 输出本次导航的所有信息
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def publish_odom_data(self, timer):
odom_tf_broadcast = TransformBroadcaster()
if (self.s7_plc.plc_connection_status() == True):
# Write to PLC motors
self.s7_plc.plc_write(self.m1_addr, self.plc_motor1_rpm)
self.s7_plc.plc_write(self.m2_addr, self.plc_motor2_rpm)
# Read Encoder Data from PLC
self.encoder1_val = self.s7_plc.plc_read(self.encoder1_addr)
self.encoder2_val = self.s7_plc.plc_read(self.encoder2_addr)
else:
rospy.loginfo("Not Connected to PLC")
if (self.encoder1_val > 125):
self.encoder1_val = self.encoder1_val - 2**32
if (self.encoder2_val > 125):
self.encoder2_val = self.encoder2_val - 2**32
encoder = encoder_data()
encoder.stamp = rospy.Time.now()
encoder.right = self.encoder1_val
encoder.left = self.encoder2_val
self.encoder_pub.publish(encoder)
# Call function to convert encoder values to linear and angular velocities
v_x, v_y, w = self._encoder_to_odometry()
# Set the current time and last recorded time
current_time = rospy.Time.now()
# compute odometry information
dt = (current_time - self.last_time).to_sec()
dx = (v_x * math.cos(self.pose.theta) -
v_y * math.sin(self.pose.theta)) * dt
dy = (v_x * math.sin(self.pose.theta) +
v_y * math.cos(self.pose.theta)) * dt
dtheta = w * dt
self.pose.x = self.pose.x + dx
self.pose.y = self.pose.y + dy
self.pose.theta = self.pose.theta + dtheta
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = transformations.quaternion_from_euler(
0, 0, self.pose.theta)
# publish the transformation on tf
odom_tf_broadcast.sendTransform((self.pose.x, self.pose.y, 0),
odom_quat, current_time, "base_link",
"odom")
# publish odometry message over ROS
odom_data = Odometry()
odom_data.header.stamp = current_time
odom_data.header.frame_id = "odom"
# set the position
odom_data.pose.pose = Pose(Point(self.pose.x, self.pose.y, 0.),
Quaternion(*odom_quat))
# set the velocity
odom_data.child_frame_id = "base_link"
odom_data.twist.twist = Twist(Vector3(v_x, v_y, 0), Vector3(0, 0, w))
# publish the message
self.odom_pub.publish(odom_data)
self.last_time = current_time
def make_pr2_gripper_marker(ps,
color,
orientation=None,
marker_array=None,
control=None,
mesh_id_start=0,
ns="pr2_gripper",
offset=-0.19):
mesh_id = mesh_id_start
# this is the palm piece
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.type = Marker.MESH_RESOURCE
if orientation != None:
mesh.pose.orientation = Quaternion(*orientation)
else:
mesh.pose.orientation = Quaternion(0, 0, 0, 1)
mesh.pose.position.x = offset
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.pose.position = ps.pose.position
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
# amount to open the gripper for each finger
angle_open = 0.4
if orientation == None:
rot0 = np.matrix(np.eye(3))
else:
rot0 = tr.quaternion_to_matrix(orientation)
if marker_array != None:
T0 = tr.composeHomogeneousTransform(
rot0, [ps.point.x, ps.point.y, ps.point.z])
else:
T0 = tr.composeHomogeneousTransform(rot0, [offset, 0.0, 0.])
#transforming the left finger and finger tip
rot1 = tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, 0.01, 0.])
rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0 * T1
T_distal = T0 * T1 * T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the left finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.color = ColorRGBA(*color)
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
#transforming the right finger and finger tip
rot1 = tr.rot_angle_direction(3.14, np.matrix(
[1, 0, 0])) * tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, -0.01, 0.])
rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0 * T1
T_distal = T0 * T1 * T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the right finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
if control != None:
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
elif marker_array != None:
return marker_array
def run(self):
pos = Point()
quat = Quaternion()
## confirm = False
## while not confirm:
## print "Current pose"
## print self.getEndeffectorPose()
## ans = raw_input("Enter y to confirm to start: ")
## if ans == 'y':
## confirm = True
## print self.getJointAngles()
#--------------------------------------------------------------
# Microwave or cabinent closing
#--------------------------------------------------------------
## print "MOVES1"
## (pos.x, pos.y, pos.z) = (0.56, -0.59, -0.24)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 2.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES2"
## (pos.x, pos.y, pos.z) = (0.67, -0.63, -0.24)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## ## raw_input("Enter anything to start: ")
## print "MOVES3"
## (pos.x, pos.y, pos.z) = (0.58, -0.56, -0.24)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 2.0
## self.setOrientGoal(pos, quat, timeout)
## # Front
## print "MOVES4"
## (pos.x, pos.y, pos.z) = (0.56, -0.59, -0.24)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 3.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES1"
## (pos.x, pos.y, pos.z) = (0.39-0.15, -0.65, 0.05)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES2"
## (pos.x, pos.y, pos.z) = (0.525-0.15, -0.65, 0.05)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## ## raw_input("Enter anything to start: ")
## print "MOVES3"
## (pos.x, pos.y, pos.z) = (0.39-0.15, -0.65, 0.05)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 2.0
## self.setOrientGoal(pos, quat, timeout)
## #--------------------------------------------------------------
## # Staples
## position_step_scaling_radius: 0.05 #0.25 #meters. default=0.25
## goal_velocity_for_hand: 0.60 #meters/sec. default=0.5
## #--------------------------------------------------------------
## # Up
## print "MOVES1"
## (pos.x, pos.y, pos.z) = (0.32, -0.63, -0.29)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 1.5
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES2"
## (pos.x, pos.y, pos.z) = (0.35+0.07, -0.63, -0.29)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 2.0
## self.setOrientGoal(pos, quat, timeout)
## ## # Front
## print "MOVES3"
## (pos.x, pos.y, pos.z) = (0.35, -0.63, -0.29)
## (quat.x, quat.y, quat.z, quat.w) = (0.0, 0.0, 0.0, 1.0)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## #--------------------------------------------------------------
## # key
## #--------------------------------------------------------------
## # Up
## print "MOVES1"
## (pos.x, pos.y, pos.z) = (0.38, -0.59, -0.44)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES2"
## (pos.x, pos.y, pos.z) = (0.38, -0.59, -0.467)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 0.3
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES3"
## (pos.x, pos.y, pos.z) = (0.38, -0.59, -0.44)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 0.3
## self.setOrientGoal(pos, quat, timeout)
## #--------------------------------------------------------------
## # case
## #--------------------------------------------------------------
## # Up
## print "MOVES1"
## (pos.x, pos.y, pos.z) = (0.42, -0.48, -0.35)
## (quat.x, quat.y, quat.z, quat.w) = (-0.146, 0.675, 0.172, 0.702)
## timeout = 4.0
## self.setOrientGoal(pos, quat, timeout)
## ## print self.getEndeffectorPose()
## ## raw_input("Enter anything to start: ")
## print "MOVES2"
## (pos.x, pos.y, pos.z) = (0.42, -0.48, -0.45)
## (quat.x, quat.y, quat.z, quat.w) = (-0.146, 0.675, 0.172, 0.702)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES3"
## (pos.x, pos.y, pos.z) = (0.44, -0.48, -0.33)
## (quat.x, quat.y, quat.z, quat.w) = (-0.146, 0.675, 0.172, 0.702)
## timeout = 3.0
## self.setOrientGoal(pos, quat, timeout)
## #--------------------------------------------------------------
## # Switch
## position_step_scaling_radius: 0.05 #0.25 #meters. default=0.25
## goal_velocity_for_hand: 0.60 #meters/sec. default=0.5
## #--------------------------------------------------------------
## print "MOVES1"
## (pos.x, pos.y, pos.z) = (0.38, -0.59, -0.42)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## ## print self.getEndeffectorPose()
## ## raw_input("Enter anything to start: ")
## print "MOVES2"
## (pos.x, pos.y, pos.z) = (0.38, -0.59, -0.445)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES3"
## (pos.x, pos.y, pos.z) = (0.38, -0.59, -0.42)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## #--------------------------------------------------------------
## # Switch -wall
## position_step_scaling_radius: 0.05 #0.25 #meters. default=0.25
## goal_velocity_for_hand: 0.60 #meters/sec. default=0.5
## #--------------------------------------------------------------
print "MOVES1"
(pos.x, pos.y, pos.z) = (0.63 - 0.15, -0.52, 0.19 - 0.3)
(quat.x, quat.y, quat.z, quat.w) = (0.849, -0.019, 0.526, 0.026)
timeout = 1.0
self.setOrientGoal(pos, quat, timeout)
## print self.getEndeffectorPose()
## raw_input("Enter anything to start: ")
print "MOVES2"
(pos.x, pos.y, pos.z) = (0.63 - 0.15, -0.52, 0.225 - 0.3)
(quat.x, quat.y, quat.z, quat.w) = (0.849, -0.019, 0.526, 0.026)
timeout = 1.0
self.setOrientGoal(pos, quat, timeout)
print "MOVES3"
(pos.x, pos.y, pos.z) = (0.63 - 0.15, -0.52, 0.19 - 0.3)
(quat.x, quat.y, quat.z, quat.w) = (0.849, -0.019, 0.526, 0.026)
timeout = 1.0
self.setOrientGoal(pos, quat, timeout)
## #--------------------------------------------------------------
## # Toaster
## position_step_scaling_radius: 0.05 #0.25 #meters. default=0.25
## goal_velocity_for_hand: 0.60 #meters/sec. default=0.5
## #--------------------------------------------------------------
## print "MOVES1"
## (pos.x, pos.y, pos.z) = (0.3, -0.59, -0.36)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES2"
## (pos.x, pos.y, pos.z) = (0.3, -0.59, -0.44)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
## print "MOVES3"
## (pos.x, pos.y, pos.z) = (0.3, -0.59, -0.36)
## (quat.x, quat.y, quat.z, quat.w) = (0.745, -0.034, -0.666, -0.011)
## timeout = 1.0
## self.setOrientGoal(pos, quat, timeout)
return True
def make_6dof_gripper(fixed,
ps,
scale,
color,
robot_type="pr2",
ignore_rotation=False,
ignore_x=False,
ignore_y=False,
ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'gripper_6dof'
control = InteractiveMarkerControl()
control.always_visible = True
control.name = 'pr2_gripper_control'
if robot_type == "pr2":
control = make_pr2_gripper_marker(ps, [0.3, 0.3, 0.3, 0.7],
control=control)
int_marker.description = 'pr2_gripper_control'
elif robot_type == "cody":
control = make_cody_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'cody_ee_control'
elif robot_type == "darci":
control = make_darci_ee_marker(ps, [1, 1, 1, 0.4], control=control)
int_marker.description = 'darci_ee_control'
int_marker.controls.append(control)
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
def execute_cb(self, goal):
goalReached=0
rate = rospy.Rate(1.0)
# goal==1 means do the fine tuning action
if (goal.goalState==1):
idealTurn = Twist()
targetAngle=0.0
robotController = rospy.Publisher('cmd_vel',Twist,queue_size=10)
while (not rospy.is_shutdown()) and (goalReached == 0):
edgeCounter = 0
for i in range(len(scan.ranges)):
if (i>0) and (abs(scan.ranges[i] < cubeThresh)):
if (scan.ranges[i-1]-scan.ranges[i]) > 0.5:
firstEdgei = i
rospy.loginfo("first edge")
firstEdgeAngle = scan.angle_min + i*scan.angle_increment
edgeCounter = edgeCounter + 1
elif (scan.ranges[i+1]-scan.ranges[i]) > 0.5:
secondEdgei = i
rospy.loginfo("second edge")
secondEdgeAngle = scan.angle_min + i*scan.angle_increment
edgeCounter = edgeCounter + 1
if (edgeCounter == 2):
blockCenterAngle = (firstEdgeAngle+secondEdgeAngle)/2.0
if (blockCenterAngle > 0) and (abs(blockCenterAngle) > np.pi/50):
idealTurn.angular.z = 0.05
rospy.loginfo("turn left")
elif (blockCenterAngle < 0) and (abs(blockCenterAngle) > np.pi/50):
idealTurn.angular.z = -0.05
rospy.loginfo("turn right")
else:
idealTurn.angular.z = 0.0
goalReached = 1
rospy.loginfo("centered")
else:
rospy.loginfo("no block center calculated")
not_norm_w = 0.0
rospy.loginfo(idealTurn.angular.z)
rospy.loginfo(edgeCounter)
robotController.publish(idealTurn)
rate.sleep()
# if the goal is anything else, move the base forward a little
else:
robotController2 = rospy.Publisher('move_base_simple/goal',PoseStamped,queue_size=10,latch=True)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "odom"
pose.pose.position.x=0.0
pose.pose.position.y=0.0
pose.pose.position.z=0.0
q = np.array([0,0,1,np.pi])
qn = q/np.linalg.norm(q)
pose.pose.orientation=Quaternion(*qn)
print pose
robotController2.publish(pose)
rospy.loginfo("pose should have published")
self._result.successOrNot=1
goalReached=1
if (goalReached):
self._result.successOrNot=1
rospy.loginfo("action success")
self._as.set_succeeded(self._result)
else:
self._result.successOrNot=0
def make_marker_flexible(fixed,
ps,
scale,
color,
mtype,
ignore_rotation,
ignore_x=False,
ignore_y=False,
ignore_z=False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = ps.header.frame_id
int_marker.pose = ps.pose
int_marker.scale = scale
int_marker.name = 'simple_6dof'
int_marker.description = ''
# insert a marker
control = InteractiveMarkerControl()
control.always_visible = True
control.markers.append(make_marker(scale, color, mtype))
int_marker.controls.append(control)
if fixed:
int_marker.name += '_fixed'
int_marker.description += '\n(fixed orientation)'
if not ignore_x:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_y:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_z:
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
if not ignore_rotation:
control = InteractiveMarkerControl()
control.orientation = Quaternion(1, 0, 0, 1)
control.name = 'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 1, 1)
control.name = 'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 1, 0, 1)
control.name = 'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
return int_marker
def plan(self, request):
"""Call Path planner selected(RRT)
Args:
request: Request by ros client. Contains start , goal , obstacle_list as:
geometry_msg/Point32 start
float32 x
float32 y
float32 z
geometry_msg/Point32 goal
float32 x
float32 y
float32 z
navigation/PolyArray obstacle_list
navigation/PointArray[] polygons
navigation/Point32[] points
float32 x
float32 y
float32 z
Returns:
RETURN_RESP: PlannerResponse()
navigation/PointArray path
navigation/Point32[] points
float32 x
float32 y
float32 z
bool ack
"""
rospy.loginfo("Planning Request received")
# Converting request from ros msg format -> basic python data type
ST_PT = request.start.x, request.start.y
END_PT = request.goal.x, request.goal.y
ROBST = request.obstacle_list.polygons
OBSTACLE = []
# Extracting Obstacle information from ROBST
for pt_array in ROBST:
tmp = []
tmp = [(pt.x, pt.y) for pt in pt_array.points]
OBSTACLE.append(tmp)
RETURN_RESP = PlannerResponse()
print("-" * 30)
rospy.loginfo(" Starting to plan from %r -> %r \n" % (ST_PT, END_PT))
print("-" * 30)
# Make class instance and get path,optimized_path
tree = RRT(sample_area=(-5, 15), sampler=sampler, expand_dis=0.1)
PATH, node_list = tree(ST_PT, END_PT, OBSTACLE)
OPTIMIZED_PATH = path_optimizer(PATH, OBSTACLE)
# print(OPTIMIZED_PATH)
# Convert optimized path to Ros Format and Send
path_to_be_published = Path()
path_to_be_published.header = Header(frame_id='odom')
path_to_be_published.poses = [
PoseStamped(pose=Pose(position=Point(x=p[0] + self.subs.get_x(),
y=p[1] + self.subs.get_y(),
z=0),
orientation=Quaternion(x=0, y=0, z=0, w=0)),
header=Header(frame_id="odom")) for p in OPTIMIZED_PATH
]
# print(path_to_be_published)
self.planning_pub.publish(path_to_be_published)
RETURN_RESP.path.points = [
Point32(x=p[0], y=p[1]) for p in OPTIMIZED_PATH
]
RETURN_RESP.ack = True
print("-" * 30)
return RETURN_RESP
def __init__(self):
rospy.init_node('position_nav_node', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 3)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
#
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
#locations['one-1'] = Pose(Point(1.1952,-0.1422,0.0), Quaternion(0.0,0.0,0.1744,0.9847))
#locations['one'] = Pose(Point(1.86,-1.247,0.0), Quaternion(0.0,0.0,0.8481,0.5298))
# locations['two-1'] = Pose(Point(1.3851,-0.1253,0.0), Quaternion(0.0,0.0,0.9818,-0.1901))
# locations['two-2'] = Pose(Point(-0.4032,-0.9106,0.0), Quaternion(0.0,0.0,-0.5523,0.8337))
# locations['two'] = Pose(Point(-0.0019,-1.9595,0.0), Quaternion(0.0,0.0,-0.5236,0.8520))
# locations['three-1'] = Pose(Point(-0.4032,-0.9106,0.0), Quaternion(0.0,0.0,-0.5523,0.8337))
# locations['three-2'] = Pose(Point(0.2382,0.0559,0.0), Quaternion(0.0,0.0,0.2464,0.9692))
locations['three'] = Pose(Point(1.0602, 0.5279, 0.0),
Quaternion(0.0, 0.0, 0.2534, 0.9674))
# locations['four'] = Pose(Point(1.0602,0.5279,0.0), Quaternion(0.0,0.0,0.2534,0.9674))
locations['three-2'] = Pose(Point(0.2382, 0.0559, 0.0),
Quaternion(0.0, 0.0, 0.2464,
0.9692)) #initialpose
locations['initial'] = locations['three-2']
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.IOTnet_pub = rospy.Publisher('/IOT_cmd', IOTnet, queue_size=10)
self.initial_pub = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=10)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = 0
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
sequeue = ['three']
# Get the initial pose from the user
#rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
#rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
setpose = PoseWithCovarianceStamped(
Header(0, rospy.Time(), "map"),
PoseWithCovariance(locations['initial'], [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.06853891945200942
]))
self.initial_pub.publish(setpose)
# Make sure we have the initial pose
rospy.sleep(1)
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.sleep(1)
# locations['back'] = Pose()
rospy.loginfo("Starting position navigation ")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the all sequence, then exit
if i == n_locations:
rospy.logwarn("Now reach all destination, now exit...")
rospy.signal_shutdown('Quit')
break
# Get the next location in the current sequence
location = sequeue[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal) #move_base.send_goal()
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Map to 4 point nav
cur_position = -1
position_seq = ['one']
if str(location) in position_seq:
cur_position = position_seq.index(str(location)) + 1
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal() #move_base.cancle_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state() #move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
# if cur_position!=-1:
# self.IOTnet_pub.publish(cur_position)
# rospy.sleep(12)
# if cur_position == 2:
# rospy.sleep(5)
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# if cur_position != -1:
# self.IOTnet_pub.publish(cur_position)
# rospy.sleep(12)
# if cur_position == 2:
# rospy.sleep(5)
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 2)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['1'] = Pose(Point(45.600, -12.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['2'] = Pose(Point(60.500, -12.000, 0.000), Quaternion(0.000, 0.000, -0.702, 0.712))
locations['3'] = Pose(Point(60.500, -12.500, 0.000), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['4'] = Pose(Point(45.600, -12.500, 0.000), Quaternion(0.000, 0.000, -0.704, 0.710))
locations['5'] = Pose(Point(45.600, -13.500, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['6'] = Pose(Point(60.500, -13.500, 0.000), Quaternion(0.000, 0.000, -0.702, 0.712))
locations['7'] = Pose(Point(60.500, -14.500, 0.000), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['8'] = Pose(Point(45.600, -14.500, 0.000), Quaternion(0.000, 0.000, -0.704, 0.710))
locations['9'] = Pose(Point(45.600, -15.500, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['10'] = Pose(Point(60.500, -15.500, 0.000), Quaternion(0.000, 0.000, -0.702, 0.712))
locations['11'] = Pose(Point(60.500, -16.500, 0.000), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['12'] = Pose(Point(45.600, -16.500, 0.000), Quaternion(0.000, 0.000, -0.704, 0.710))
locations['13'] = Pose(Point(45.600, -17.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['14'] = Pose(Point(60.500, -17.000, 0.000), Quaternion(0.000, 0.000, 1.000, 0.002))
#locations['midten'] = Pose(Point(-42.651, 24.841, 0.000), Quaternion(0.000, 0.000,-0.734, 0.679))
#locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
#rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
#rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
#self.last_location = Pose()
#rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
# Make sure we have the initial pose
#while initial_pose.header.stamp == "":
# rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = ['1','2','3','4','5','6','7','8','9','10','11','12','13','14']
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
print sequence
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def publish_estimates(self, timestamp, last_orientation_quat,
orientation_cov):
ne = NetworkEstimate()
for asset in self.blue_agent_names:
ind = self.blue_agent_names.index(asset)
x_hat_agent, P_agent, _ = self.kf.get_agent_states(ind)
pose_cov = np.zeros((6, 6))
pose_cov[:3, :3] = P_agent[:3, :3]
if asset == self.my_name:
pose = Pose(
Point(x_hat_agent[0], x_hat_agent[1], x_hat_agent[2]),
last_orientation_quat)
pose_cov[3:, 3:] = orientation_cov[3:, 3:]
elif "red" in asset:
pose_cov = 5 * np.eye(6) # Just set single uncertainty
red_agent_depth = -0.7
pose = Pose(
Point(x_hat_agent[0], x_hat_agent[1], red_agent_depth),
Quaternion(0, 0, 0, 1))
pose_cov[3:, 3:] = -np.eye(3)
else:
pose = Pose(
Point(x_hat_agent[0], x_hat_agent[1], x_hat_agent[2]),
Quaternion(0, 0, 0, 1))
pose_cov[3:, 3:] = -np.eye(3)
pwc = PoseWithCovariance(pose, list(pose_cov.flatten()))
twist_cov = -np.eye(6)
twist_cov[:3, :3] = P_agent[3:6, 3:6]
tw = Twist()
tw.linear = Vector3(x_hat_agent[3], x_hat_agent[4], x_hat_agent[5])
twc = TwistWithCovariance(tw, list(twist_cov.flatten()))
h = Header(self.update_seq, timestamp, "odom")
o = Odometry(h, "odom", pwc, twc)
ae = AssetEstimate(o, asset)
ne.assets.append(ae)
self.asset_pub_dict[asset].publish(o)
if self.red_agent_exists:
asset = self.red_agent_name
ind = self.blue_agent_names.index(asset)
x_hat_agent, P_agent = self.kf.get_agent_states(ind)
pose_cov[:3, :3] = P_agent[:3, :3]
red_agent_depth = -0.7
pose = Pose(Point(x_hat_agent[0], x_hat_agent[1], red_agent_depth),
Quaternion(0, 0, 0, 1))
pose_cov[3:, 3:] = -np.eye(3)
pwc = PoseWithCovariance(pose, list(pose_cov.flatten()))
twist_cov = -np.eye((6, 6))
twist_cov[:3, :3] = P_agent[3:6, 3:6]
tw = Twist()
tw.linear = Vector3(x_hat_agent[3], x_hat_agent[4], x_hat_agent[5])
twc = TwistWithCovariance(tw, list(twist_cov.flatten()))
h = Header(self.update_seq, timestamp, "odom")
o = Odometry(h, "odom", pwc, twc)
ae = AssetEstimate(o, asset)
ne.assets.append(ae)
self.asset_pub_dict[asset].publish(o)
self.network_pub.publish(ne)
def show_results(img, results):
view = img.copy()
rospy.init_node('nav_test', anonymous=False)
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
for i in range(len(results)):
x = int(results[i][1])
y = int(results[i][2])
w = int(results[i][3]) * 3 #3 1 +100
h1 = int(results[i][4]) // 2 # //4
#x1 = int(results[0][1])
#y1 = int(results[0][2])
#w1 = int(results[0][3])//3
#h1 = int(results[0][4])+800
#results[0][5] = results[0][5] * .8
# x = abs(x)
# y = abs(y)
w = w + 30
h = int(h1) * 6
#w = w * 4 #with +100
#w = w // 2# with +100
#count = i+1
results[i][5] = results[i][5] * 3.1 + (h1 * 0.15)
locations = dict()
locations['lab1'] = Pose(Point(51.967, 45.673, 0.000),
Quaternion(0.000, 0.000, -0.670, 0.743))
locations1 = dict()
locations1['lab1'] = Pose(Point(52.041, 42.851, 0.000),
Quaternion(0.000, 0.000, -0.670, 0.743))
if results[i][0] == 'right':
# for count in range(1,100):
count1 = 0
global count
count = count + 1
print count
if count > 50:
print(results[i][0] + 'start')
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map' # base_link
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = locations['lab1']
# goal.target_pose.pose.position.x = 0.5 #3.0 3 meters
# goal.target_pose.pose.orientation.w = 1.0 #1.0 go forward
#start moving
move_base.send_goal(goal)
elif results[i][0] == 'left':
global count1
count = 0
count1 += 1
print count1
if count1 > 50:
print(results[i][0] + 'start')
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map' # base_link
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = locations1['lab1']
#start moving
move_base.send_goal(goal)
if display:
print(' class : ' + results[i][0] + ' , [x,y,w,h]=[' + str(x) +
',' + str(y) + ',' + str(int(results[i][3])) + ',' +
str(int(results[i][4])) + '], Confidence = ' +
str(results[i][5]))
if results[i][0] == 'left' and int(threshold) <= 19.4:
count = 0
#x = x + 1
#y = y + 1
h = int(h) // 10 #380
#h = h*2
w = int(w) * 5 #240
#w = w*2
print(' class : ' + results[0][0] + ' , [x,y,w,h]=[' +
str(x) + ',' + str(y) + ',' + str(int(results[0][3])) +
',' + str(int(results[0][4])) + '], Confidence = ' +
str(results[0][5]))
if imshow:
cv2.rectangle(view, (x - w, y - h), (x + w, y + h), (0, 255, 0),
5) # 0 255 0 2
cv2.putText(view, 'Detecting: ' + results[i][0] +
' - %.2f' % results[i][5] + '%',
(x - w + 5, y - h + 350), cv2.FONT_HERSHEY_SIMPLEX,
0.8, (0, 0, 255), 2) # +5 -7 #0.5 000 1 #
if results[i][0] == 'left':
cv2.rectangle(view, (x - w, y - h), (x + w, y + h),
(0, 255, 255), 5) # 0 255 0 2
# cv2.rectangle(view,(x-w,y-h-20),(x+w,y-h),(125,125,125),-5) # 125 125 125 -1
cv2.putText(view, 'Detecting: ' + results[0][0] +
' - %.2f' % results[0][5] + '%',
(x - w + 5, y - h + 100), cv2.FONT_HERSHEY_SIMPLEX,
0.8, (0, 255, 255), 2) # +5 -7 #0.5 000 1 #
# if self.filewrite_txt :
# ftxt.write(results[i][0] + ',' + str(x) + ',' + str(y) + ',' + str(w) + ',' + str(h)+',' + str(results[i][5]) + '\n')
# if self.filewrite_img :
# if self.display : print (' image file writed : ' + self.tofile_img)
# cv2.imwrite(self.tofile_img,view)
if imshow:
#cv2.namedWindow("detection", cv2.WND_PROP_FULLSCREEN)
#cv2.setWindowProperty("detection",cv2.WND_PROP_FULLSCREEN,cv2.WINDOW_FULLSCREEN)
cv2.namedWindow("detection", cv2.WINDOW_NORMAL) #WINDOW_AUTOSIZE
#cv2.setWindowProperty("detection",cv2.WND_PROP_AUTOSIZE,cv2.WINDOW_AUTOSIZE)
cv2.imshow('detection', view)
cv2.waitKey(1)
# if self.filewrite_txt :
# if self.display : print (' txt file writed : ' + self.tofile_txt)
# ftxt.close()
return results
def heading(self, yaw):
q = quaternion_from_euler(0, 0, yaw)
return Quaternion(*q)
def callback(self, bearing):
"""This callback occurs whenever the object detection script publishes
to the /detection topic. If the array is [0,0], no object was detected
and we move the UGV along the pre-defined search routine. If the array
is not [0,0], we move the UGV toward the object.
"""
def back_it_up(ve, dist_to_move):
"""This subroutine manually moves the husky forward or backward
a fixed amount at a fixed velocity by bypassing the move_base
package and sending a signal directly to the wheels.
This subroutine is likely to be replaced by:
file:
robot_mv_cmds
subroutine: move_UGV_vel(lv,av,dist_to_move)
"""
sleep_time = 0.1
time_to_move = abs(dist_to_move / ve)
twist = Twist()
twist.linear.x = ve
self.ct_move = 0
while self.ct_move * sleep_time < time_to_move:
self.twi_pub.publish(twist)
self.ct_move = self.ct_move + 1
rospy.sleep(sleep_time)
return None
def rot_cmd(ve, dist_to_move):
"""This subroutine manually rotates the husky along the z-axis a
fixed amount at a fixed velocity by bypassing the move_base package
and sending a signal directly to the wheels.
This subroutine is likely to be replaced by:
file:
robot_mv_cmds
subroutine: move_UGV_vel(lv,av,dist_to_move)
"""
sleep_time = 0.1
time_to_move = abs(dist_to_move / ve)
twist = Twist()
twist.angular.z = ve
self.ct_move = 0
while self.ct_move * sleep_time < time_to_move:
self.twi_pub.publish(twist)
self.ct_move = self.ct_move + 1
rospy.sleep(sleep_time)
return None
if bearing.data[0] == 0:
"""If /detection topic is publishing [0,0] then we do not see an
object. Move along the predefined path.
"""
try:
self.state = self.move_base.get_state()
except:
pass
self.noise_counter = self.noise_counter + 1
rospy.logdebug("I see no object!")
if self.state == 3:
# If move_base is registering 'SUCCEEDED' move to next waypoint
self.current_waypoint = self.current_waypoint + 1
location = self.waypoint_name[self.current_waypoint]
self.goal.target_pose.pose = self.locations[location]
self.goal.target_pose.header.frame_id = 'odom'
rospy.loginfo("Going to: " + str(location))
self.move_base.send_goal(self.goal)
print "Going to: "
print self.goal
self.stall_counter = 0
self.detect_counter = 0
rospy.sleep(2)
else:
# If not registering 'SUCCEEDED', increment stall_counter
self.stall_counter = self.stall_counter + 1
if self.stall_counter > self.stalled_threshold:
# If stall_counter is too high, enter stuck routine.
# Ideally this will be replaced with a catch routine in
# husky_navigation package.
rospy.loginfo("We are stuck!")
rospy.loginfo("Applying catch routine.")
self.move_base.cancel_goal()
rospy.sleep(0.1)
back_it_up(-0.25, 0.5)
rospy.sleep(0.1)
rot_cmd(0.25, 0.25)
rospy.loginfo("Resending goal.")
print "Resending goal: "
print self.goal
self.move_base.send_goal(self.goal)
rospy.sleep(1)
self.ct_move = 0
self.stall_counter = 0
if self.noise_counter > 5:
# Check if we have gotten more blank scans than allowed. Reset
# the detection counter.
self.detect_counter = 0
else:
"""If /detection is not publishing [0,0], we found an object! To
remove noisy signals, we make sure we find 10 valid scans.
"""
if self.detect_counter > 10:
# Check number of valid scans to identify the object.
rospy.logdebug("Object identified.")
# move_base doesn't like targets that are too far away. If the
# range is too far away, fix the range to 10m.
if bearing.data[1] > 10:
bear = 10
else:
bear = bearing.data[1]
# If the UGV is within 3m of the target, set the goal to 0
if bear < 5:
bear = 0
# Create target location in the local reference frame from the
# /detection angle and range
x = bear * np.cos(bearing.data[0]) - 1.5
y = bear * np.sin(bearing.data[0]) + 0.5
self.goal.target_pose.header.frame_id = 'base_link'
q = tf.transformations.quaternion_from_euler(
0, 0, bearing.data[0])
if abs(bearing.data[0]) > 3 * 1.57:
self.goal.target_pose.pose = Pose(
Point(x * 0.0, y * 0, 0),
Quaternion(q[0], q[1], q[2], q[3]))
rospy.loginfo("Going to (x,y,yaw): (%f,%f,%f)", x * 0.1,
y * 0.1, bearing.data[0])
else:
self.goal.target_pose.pose = Pose(
Point(x, y, 0), Quaternion(q[0], q[1], q[2], q[3]))
rospy.loginfo("Going to (x,y,yaw): (%f,%f,%f)", x, y,
bearing.data[0])
self.goal.target_pose.header.stamp = rospy.Time.now()
self.move_base.send_goal(self.goal)
try:
self.state = self.move_base.get_state()
except:
pass
self.detect_counter = self.detect_counter + 1
if bear < 3:
rospy.set_param('smach_state', 'atBoard')
rospy.signal_shutdown('We are close enough to the object!')
rospy.sleep(2)
rospy.loginfo("State:" + str(self.state))
else:
self.detect_counter = self.detect_counter + 1
self.noise_counter = 0
def update_odom(self):
t2 = rospy.Time.now()
t1 = self.prev_time
dt = (t2 - t1).to_sec()
gyro_thresh = 0.01 #0.05
g_bias = 0.007
MAX_DTHETA_GYRO = 5
try:
self.ardIMU.safe_write('A5/6/')
gyroz = self.ardIMU.safe_read()
#c = 0
#print("gyro z:")
#print(gyroz)
#while(gyroz == '' and c < 10):
# c = c+1
# gyroz = ardIMU.safe_read()
# print('try gyro again')
gyroz = float(int(gyroz) - 1000) / 100.0
except:
gyroz = 0
print 'IMU error'
print "Unexpected Error:", sys.exc_info()[0]
finally:
a = 0
# Read encoder delta
try:
self.ard.safe_write('A3/4/')
s = self.ard.safe_read()
#print("enc: ")
#print(s)
c = 0
#while(s == '' and c < 10):
# c = c +1
# s = ardIMU.safe_read()
# print('try enc again')
delta_enc_counts = int(s)
except:
delta_enc_counts = 0
print 'enc error'
print "Unexpected Error:", sys.exc_info()[0]
finally:
a = 0
# Update odom
self.enc_total = self.enc_total + delta_enc_counts
dmeters = float(delta_enc_counts) / 53.0 #53 counts/meter
if (abs(gyroz + g_bias) < gyro_thresh):
gz_dps = 0
dtheta_gyro_deg = 0
else:
gz_dps = (gyroz + g_bias) * 180 / 3.14
#if(gz_dps > 0):
# gz_dps = gz_dps * 1.2
#else:
# gz_dps = gz_dps * 1.1
dtheta_gyro_deg = gz_dps * dt
if (abs(dtheta_gyro_deg) > MAX_DTHETA_GYRO):
#print 'no gyro'
dtheta_deg = 0
use_gyro_flag = False
else:
#print 'use gyro'
dtheta_deg = dtheta_gyro_deg
use_gyro_flag = True
#update bot position
self.bot.move(dmeters, dtheta_deg, use_gyro_flag)
self.bot.servo_deg = 0
# update bot linear x velocity every 150 msec
# need to use an np array, then push and pop, moving average
self.dist_sum = self.dist_sum + dmeters
self.time_sum = self.time_sum + dt
if (self.time_sum > 0.15):
self.vx = self.dist_sum / self.time_sum
self.dist_sum = 0
self.time_sum = 0
#bot.botx*100,bot.boty*100,bot.bot_deg
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.bot.bot_deg * 3.14 / 180.0)
self.odom_broadcaster.sendTransform((self.bot.botx, self.bot.boty, 0.),
odom_quat, t2, "base_link", "odom")
odom = Odometry()
odom.header.stamp = t2
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(self.bot.botx, self.bot.boty, 0.),
Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(self.vx, 0, 0),
Vector3(0, 0, gz_dps * 3.14 / 180.0))
# publish the message
self.odom_pub.publish(odom)
self.prev_time = t2
