def cbNextGoal(self, msg):
debug = False
# Pop next goal on path
try:
# If we can pop another goal
rospy.loginfo("[%s] reach destination, pop next goal" %
(self.node_name))
self.goal = self.path.pop()
if not debug:
goal_msg = Pose2DStamped()
goal_msg.header.stamp = rospy.Time.now()
goal_msg.x = self.goal[0]
goal_msg.y = self.goal[1]
goal_msg.theta = self.goal[2]
self.pub_goal.publish(goal_msg)
#self.Rate.sleep()
except:
# Cannot pop another goal, reach destination
if debug:
self.path = [(0.0, 0.0, 0.0), (1000.0, 0.0, 0.0),
(1000.0, 1000.0, 0.0), (0.0, 1000.0, 0.0)]
else:
reach_msg = BoolStamped()
reach_msg.header.stamp = rospy.Time.now()
reach_msg.data = True
self.pub_reach_dest.publish(reach_msg)
def cbSrvJoyOff(self, req):
# joystick override True button
joy_override_msg = BoolStamped()
joy_override_msg.header.stamp = rospy.Time.now()
joy_override_msg.data = False
self.pub_buttons.publish(joy_override_msg)
return EmptyResponse()
def cbSrvFind(self, req):
msg = BoolStamped()
msg.data = True
self.finish_flag = False
self.cbTrigger(msg)
while (1):
return EmptyResponse()
def cbSrvReach(self, req):
reach_msg = BoolStamped()
reach_msg.header.stamp = rospy.Time.now()
reach_msg.data = True
self.pub_reach.publish(reach_msg)
self.active = False
return EmptyResponse()
def pub_confrim_msg(self):
# Finish planning, set fsm to next state
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_confrim.publish(finish_msg)
# set status buffer
self.move_ready = False
self.path_ready = False
def cbFinishCoord(self, msg):
return
if not self.active:
return
self.fetch()
# Publish confirm message to task planner
conf_msg = BoolStamped()
conf_msg.header.stamp = rospy.Time.now()
conf_msg.data = True
self.pub_finish.publish(conf_msg)
def checkProtocol(self, tag_id):
# A fake protocol for test
if tag_id == 1:
task_success = True
else:
task_success = False
coord_msg = BoolStamped()
coord_msg.header.stamp = rospy.Time.now()
coord_msg.data = task_success
self.pub_coord.publish(coord_msg)
def cbNextmove(self,msg):
if not self.active:
return
# Pop next move
try:
# If we can pop another move
self.move = self.moves.pop()
except:
# Cannot pop another move, reach destination
reach_msg = BoolStamped()
reach_msg.header.stamp = rospy.Time.now()
reach_msg.data = True
def cbSrvMacor2(self, msg):
try:
self.macro_task_index = 2
self.macro_tasks = rospy.get_param("~macro_tasks")
self.current_macro_task = self.macro_tasks[str(
'macro_task_' + str(self.macro_task_index))][:]
self.current_task = self.current_macro_task.pop()
rospy.loginfo(
"soft reset current macro task to macrotask 2: [%s]" %
(self.current_macro_task))
except Exception as e:
rospy.loginfo(e)
# Publish finish message to enter planning state
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_reset.publish(finish_msg)
return EmptyResponse()
def publishBools(self):
active_nodes = self._getActiveNodesOfState(self.state_msg.state)
for node_name, node_pub in self.pub_dict.items():
msg = BoolStamped()
msg.header.stamp = self.state_msg.header.stamp
msg.data = bool(node_name in active_nodes)
node_state = "ON" if msg.data else "OFF"
rospy.loginfo(
"[%s] Node %s is %s in %s" %
(self.node_name, node_name, node_state, self.state_msg.state))
if self.active_nodes is not None:
if (node_name in active_nodes) == (node_name
in self.active_nodes):
continue
# else:
# rospy.logwarn("[%s] self.active_nodes is None!" %(self.node_name))
# continue
node_pub.publish(msg)
# rospy.loginfo("[%s] node %s msg %s" %(self.node_name, node_name, msg))
# rospy.loginfo("[%s] Node %s set to %s." %(self.node_name, node_name, node_state))
self.active_nodes = copy.deepcopy(active_nodes)
def cbSetMove(self, msg):
# Convert move from message
moves = []
moves.append(msg.data)
rospy.loginfo("[%s] set move: %s" %(self.node_name, msg.data))
# Set current move
self.moves = moves
self.move = self.moves.pop()
# Publish confirm message
conf_msg = BoolStamped()
conf_msg.header.stamp = rospy.Time.now()
self.pub_set_confirm.publish(conf_msg)
def cbFinishCoord(self, msg):
if not self.state == "AT_GOAL":
return
if len(self.current_macro_task) > 0:
return
# Finish coordination switch to next macro task
if msg.data:
# I receive positive confirm, switch to next task
try:
# Try to pop the next macrotask
self.macro_task_index += 1
self.macro_tasks = rospy.get_param("~macro_tasks")
self.current_macro_task = self.macro_tasks[str(
'macro_task_' + str(self.macro_task_index))][:]
rospy.loginfo("set current macro task to [%s]" %
(self.current_macro_task))
self.current_task = self.current_macro_task.pop()
except:
# Or its the last task
self.macro_task_index -= 1 # do the last macro task
self.macro_tasks = rospy.get_param("~macro_tasks")
self.current_macro_task = self.macro_tasks[str(
'macro_task_' + str(self.macro_task_index))][:]
rospy.loginfo("set current macro task to [%s]" %
(self.current_macro_task))
self.current_task = self.current_macro_task.pop()
else:
# else continue precious macro task
#print self.macro_task_index
self.macro_tasks = rospy.get_param("~macro_tasks")
self.current_macro_task = self.macro_tasks[str(
'macro_task_' + str(self.macro_task_index))][:]
self.current_task = self.current_macro_task.pop()
rospy.loginfo("set current macro task to [%s]" %
(self.current_macro_task))
# Publish finish message to enter planning state
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_finish.publish(finish_msg)
def next_task(self):
# Reset switch
self.path_finish = False
self.move_finish = False
# Pop next task
try:
self.current_task = self.current_macro_task.pop()
#self.current_task = self.macro_tasks['macro_task_0'].pop()
rospy.loginfo("[%s] pop next task [%s]" %
(self.node_name, self.current_task))
# Publish finish message
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
self.pub_finish.publish(finish_msg)
except:
debug = True
if debug:
#self.current_task = self.default_task
#rospy.loginfo("[%s] pop default task" %(self.node_name))
# Publish finish message
finish_msg = BoolStamped()
finish_msg.header.stamp = rospy.Time.now()
finish_msg.data = True
#self.pub_finish.publish(finish_msg)
self.cbFinishCoord(finish_msg)
return
def cbSetPath(self, msg):
# Convert path from message
path = []
data_list = msg.data_list
for i in range(len(data_list)):
ith_pose = data_list[i]
path.append((ith_pose.x, ith_pose.y, ith_pose.theta))
rospy.loginfo("[%s] set path point: %s" %
(self.node_name, path[i]))
# Set current path
self.path = path
self.goal = self.path.pop()
# Publish confirm message
conf_msg = BoolStamped()
conf_msg.header.stamp = rospy.Time.now()
self.pub_set_confirm.publish(conf_msg)
def cbAndSwitch(self, msg):
# joystick override False button
if msg.data == True:
joy_override_msg = BoolStamped()
joy_override_msg.header.stamp = rospy.Time.now()
joy_override_msg.data = False
self.pub_buttons.publish(joy_override_msg)
# joystick override True button
if msg.data == False:
joy_override_msg = BoolStamped()
joy_override_msg.header.stamp = rospy.Time.now()
joy_override_msg.data = True
self.pub_buttons.publish(joy_override_msg)
def processButtons(self, joy_msg):
# joystick override False button
if joy_msg.buttons[7] == True:
joy_override_msg = BoolStamped()
joy_override_msg.header.stamp = rospy.Time.now()
joy_override_msg.data = False
self.pub_buttons.publish(joy_override_msg)
# joystick override True button
if joy_msg.buttons[6] == True:
joy_override_msg = BoolStamped()
joy_override_msg.header.stamp = rospy.Time.now()
joy_override_msg.data = True
self.pub_buttons.publish(joy_override_msg)
return
def callback(self, msg):
if not self.active:
return
#print msg
tag_infos = []
trans_x_buf = 0.0
trans_y_buf = 0.0
trans_z_buf = 0.0
rot_z_buf = 0.0
yaw_buf = 0.0
# Load tag detections message
for detection in msg.detections:
tag_id = int(detection.id)
self.checkProtocol(tag_id)
# Define the transforms
veh_t_camxout = tr.translation_matrix(
(self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180,
0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(
veh_t_camxout,
veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2,
'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout,
camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2,
'rxyz')
#Load translation
trans = detection.pose.pose.position
rot = detection.pose.pose.orientation
camzout_t_tagzout = tr.translation_matrix(
(trans.x * self.scale_x, trans.y * self.scale_y,
trans.z * self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix(
(rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(
camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout,
camzout_T_tagzout,
tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y,
trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z,
rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
frame_name = '/tag' + str(tag_id)
try:
(trans_tf, rot_tf) = self.listener.lookupTransform(
'/ducky1', frame_name, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
rot_tf_rpy = tf.transformations.euler_from_quaternion(rot_tf)
yaw = rot_tf_rpy[2] / np.pi
# Check phase
if tag_id == 3:
# Back of the objet, phase confusion
if rot.z < 0.0:
# turn around the coordinate frame to avoid confusion
yaw = -yaw
# Publish tag id
tag_id_msg = Int8()
tag_id_msg.data = tag_id
self.pub_tagId.publish(tag_id_msg)
# Publish ack
ack_msg = BoolStamped()
if trans.x < 0.7:
ack_msg.data = True
else:
ack_msg.data = False
self.pub_ack.publish(ack_msg)
#print detection
print detection #trans.x, trans.y, trans.z
#self.checkProtocol(tag_id)
#print rot.z - yaw
#print tag_id, trans, rot
rot_z_buf += rot.z
yaw_buf += yaw
if len(msg.detections) > 0:
# Broadcast tranform
self.broadcaster.sendTransform(
(trans.x, trans.y, trans.z),
tf.transformations.quaternion_from_euler(
0, 0,
np.pi *
(rot_z_buf - yaw_buf) / float(len(msg.detections))),
rospy.Time.now(), "ducky1", "camera")
def verify(self, data_list, plot_everything=False):
if not self.verify_switch:
return
#if len(data_list) > 0:
#print data_list
# Display
if plot_everything:
self.display(data_list)
try:
# Check protocal
assert len(data_list) == self.data_length[0]
data = []
left_open = False
left_shoot = False
right_open = False
right_shoot = False
for raw in data_list:
datum = struct.unpack('B', "".join(raw))[0]
data.append(datum)
rospy.loginfo("revice ack info [%s]" % (datum))
if data[0] == 23 and data[1] == 33:
# Setup offset
# update robot state parameter
robot_state = rospy.get_param("/robot_state")
robot_state[1]["offset"][0] = robot_state[0]["odom"][0]
robot_state[1]["offset"][1] = robot_state[0]["odom"][1]
robot_state[1]["offset"][2] = robot_state[0]["odom"][2]
rospy.loginfo("set up offset [%s]" %
(robot_state[1]["offset"]))
rospy.set_param("/robot_state", robot_state)
elif data[0] == self.ack_codebook["left_shoot"]:
# ack signal to shoot task
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = True
self.pub_ack_left_shoot.publish(msg)
if data[0] == self.ack_codebook["left_open"]:
left_open = True
if data[1] == self.ack_codebook["right_open"]:
right_open = True
if data[2] == self.ack_codebook["left_shoot"]:
left_shoot = True
if data[3] == self.ack_codebook["right_shoot"]:
right_shoot = True
# ack signal to left fetch task
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = left_open
self.pub_ack_left_open.publish(msg)
# ack signal to left shoot task
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = left_shoot
self.pub_ack_left_shoot.publish(msg)
# ack signal to right fetch task
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = right_open
self.pub_ack_right_open.publish(msg)
# ack signal to right shoot task
msg = BoolStamped()
msg.header.stamp = rospy.Time.now()
msg.data = right_shoot
self.pub_ack_right_shoot.publish(msg)
except Exception as e:
rospy.loginfo(e)
return
# Debug message
joy_msg = Joy()
debug_0 = float(data_list[0]) / 128.0 - 1.0
debug_1 = float(data_list[1]) / 128.0 - 1.0
debug_2 = float(data_list[2]) / 128.0 - 1.0
debug_3 = float(data_list[3]) / 128.0 - 1.0
for i in range(8):
joy_msg.axes.append(0)
joy_msg.axes[4] = debug_0
joy_msg.axes[3] = debug_1
("**************************************************")
rospy.loginfo("[%s] ch 0: [%s] ch 1: [%s]" %
(self.node_name, debug_0, debug_1))
rospy.loginfo("[%s] ch 2: [%s] ch 3: [%s]" %
(self.node_name, debug_2, debug_3))
self.pub_debug.publish(joy_msg)
return
def publishMove(self):
# Publish move message
#move_msg = String()
# move_msg.data = self.move
#self.pub_move.publish(move_msg)
if self.move == 'sleep':
rospy.sleep(4)
elif self.move == 'pass':
rospy.sleep(0.01)
elif self.move == 'throw':
rospy.sleep(10)
elif self.move == 'joy':
self.set_joy()
elif self.move == 'release':
self.release()
elif self.move == 'fetch':
self.fetch()
elif self.move == 'roi':
set_msg = BoolStamped()
set_msg.header.stamp = rospy.Time.now()
self.pub_set_ref.publish(set_msg)
elif self.move == "find_circle":
self.find_circle()
elif self.move == "open_left":
if self.left_open == False:
while(self.left_open == False):
print "loop"
self.open_left()
self.cmd_rate.sleep()
elif self.move == "close_left":
if self.left_open == True:
while(self.left_open == True):
self.close_left()
elif self.move == "open_right":
if self.right_open == False:
while(self.right_open == False):
self.open_right()
elif self.move == "close_right":
if self.right_open == True:
while(self.right_open == True):
self.open_right()
elif self.move == "shoot_right":
self.right_shooting = False
while(self.right_shooting == False):
self.shoot_right()
elif self.move == "shoot_left":
self.left_shooting = False
while(self.left_shooting == False):
#self.lit()
self.shoot_left()
elif self.move == "apriltag":
self.apriltag_flag = False
while(self.apriltag_flag == False):
self.cmd_rate.sleep()
elif self.move == "wait_shoot":
if self.left_shooting or self.right_shooting:
while(self.left_shooting or self.right_shooting):
self.cmd_rate.sleep()
elif self.move == "adjust_angle":
self.adjust_angle()
elif self.move == "lit":
self.lit()
else:
rospy.sleep(1)
rospy.loginfo("[%s] send move: %s" %(self.node_name, self.move))
# Publish confirm message to task planner
conf_msg = BoolStamped()
conf_msg.header.stamp = rospy.Time.now()
conf_msg.data = True
self.pub_finish.publish(conf_msg)
def cbPose(self, msg, plot_pose=True, debug_flag=True):
if debug_flag == False:
return
# Callback for pose message from decoder node
if self.active == False:
return
if plot_pose:
rospy.loginfo("[%s]: X: %s, Y: %s, Theta: %s" %
(self.node_name, msg.x, msg.y, msg.theta))
# Extract data from message
time_stamp = msg.header.stamp
x = msg.x
y = msg.y
theta = msg.theta
# Some strange bug, need to be fixed
if x == 0.0 and y == 0.0 and theta == 0.0:
return
# Generate control effort
# error
error_x = x - self.x_goal
error_y = y - self.y_goal
#error_theta = theta - self.theta_goal
comp_theta_goal = complex(np.cos(self.theta_goal * np.pi / 180.0),
np.sin(self.theta_goal * np.pi / 180.0))
comp_theta = complex(np.cos(theta * np.pi / 180.0),
np.sin(theta * np.pi / 180.0))
delta_theta = comp_theta / comp_theta_goal
error_theta = np.angle(delta_theta) / np.pi * 180.0
# Check at goal
if abs(error_theta) < self.theta_thresh:
if abs(error_x) < 100.0 and abs(error_y) < 100.0:
# Publish car command
twist_msg = Twist2DStamped()
twist_msg.header.stamp = rospy.Time.now()
twist_msg.v_x = 0.0
twist_msg.v_y = 0.0
twist_msg.omega = 0.0
self.pub_twist.publish(twist_msg)
reach_msg = BoolStamped()
reach_msg.header.stamp = rospy.Time.now()
reach_msg.data = True
self.pub_reach.publish(reach_msg)
self.active = False
return
# for simplicity, use sign control, will add fuzzy controller
sign_control = False
if sign_control:
if error_x > 0.0:
v_x = -0.2
else:
if error_x < 0.0:
v_x = 0.2
else:
v_x = 0.0
if error_y > 0.0:
v_y = -0.2
else:
if error_y < 0.0:
v_y = 0.2
else:
v_y = 0.0
sigmoid_p_control = True
sigmoid_scalar = 2000.0
dead_zone = 20.0
if sigmoid_p_control:
print "*"
res_error_x = error_x / sigmoid_scalar
res_error_y = error_y / sigmoid_scalar
res_deadzone = 0.05
res_distance = math.sqrt(res_error_x**2 + res_error_y**2)
print res_error_x, res_error_y, res_distance
position = complex(res_error_x, res_error_y)
angle_map = np.angle(position, deg=True)
angle_car = angle_map + theta
print angle_car
#trigger = False
if res_distance < res_deadzone:
v_x = 0.0
v_y = 0.0
else:
#self.trigger = False
control_effort = self.sigmoid(res_distance)
v_x = -control_effort * np.cos(
angle_car * np.pi /
180.0) / 1.5 #(res_error_x / res_distance)
v_y = -control_effort * np.sin(
angle_car * np.pi /
180.0) / 1.5 #(res_error_y / res_distance)
# Maintain the same frame with odometry
simple_omega = False
if simple_omega:
if abs(theta - self.theta_goal) < self.theta_thresh:
omega = 0.0
else:
#v_x = 0.0
#v_y = 0.0
if theta - self.theta_goal > 0:
omega = -0.2
else:
omega = 0.2
complex_omega = True
if complex_omega:
#comp_theta_goal = complex(np.cos(self.theta_goal * np.pi / 180.0), np.sin(self.theta_goal * np.pi / 180.0))
#comp_theta = complex(np.cos(theta * np.pi / 180.0), np.sin(theta * np.pi / 180.0))
#delta_theta = comp_theta / comp_theta_goal
#error_theta = np.angle(delta_theta) / np.pi * 180.0
rospy.loginfo("[%s] error theta [%s]" %
(self.node_name, error_theta))
k_omega = 180.0
if abs(error_theta) < self.theta_thresh:
omega = 0.0
elif error_theta > 0.0:
v_x = v_x * 0.5
v_y = v_y * 0.5
omega = -abs(error_theta) / k_omega - 0.1
else:
v_x = v_x * 0.5
v_y = v_y * 0.5
omega = abs(error_theta) / k_omega + 0.1
# fuzzy controller
use_fuzzy = False
if use_fuzzy:
if abs(theta) < self.theta_thresh:
omega = 0.0
else:
v_x = 0.0
v_y = 0.0
fuzzy_system = OmegaControl()
omega_ctl = fuzzy_system.getController()
omega_ctl.input['theta_error'] = theta / 180.0
omega_ctl.compute()
omega = omega_ctl.output['out_omega']
omega = omega / 2.0
#rospy.loginfo("control effort [%s] [%s]")
# Publish car command
twist_msg = Twist2DStamped()
twist_msg.header.stamp = rospy.Time.now()
twist_msg.v_x = v_x
twist_msg.v_y = v_y
twist_msg.omega = omega
self.pub_twist.publish(twist_msg)