class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) self.end = np.array([[0.3, 2.2, 1.2, 2.2], [4.1, 3.9, 3, 3.9], [4.1, 2.2, 3, 2.2], [4.1, 0.5, 3, 0.5]]) self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.2) self.pose = None self.pub_cmd = rospy.Publisher("cmd_vel", Twist, queue_size=1) self.pub_goal_marker = rospy.Publisher( "goal_marker", Marker, queue_size=1) self.pub_pid_goal = rospy.Publisher( "pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_pose = rospy.Subscriber( "pose", PoseStamped, self.cb_pose, queue_size=1) self.srv_topos = rospy.Service("to_position", GoToPos, self.to_pos) def pub_marker(self, goal=Marker()): marker = Marker() marker.header.frame_id = "global" marker.header.stamp = rospy.Time.now() marker.ns = "look ahead" marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.g = 1.0 self.pub_goal_marker.publish(marker) def to_pos(self, req): rospy.loginfo("%s : request pos %d" % (rospy.get_name(), req.pos)) res = GoToPosResponse() if req.pos < 0 or req.pos > 3: rospy.logerr("%s : pos not exist" % rospy.get_name()) res.result = False return res if self.pose is None: rospy.logerr("%s : no pose" % rospy.get_name()) res.result = False return res my_pose = np.array([self.pose.pose.position.x, self.pose.pose.position.y]) dis = np.linalg.norm(self.end[req.pos][:2]-my_pose) # turn arround -------------------------------------------------------- if dis > 0.5: while not rospy.is_shutdown(): quat = (self.pose.pose.orientation.x, self.pose.pose.orientation.y, self.pose.pose.orientation.z, self.pose.pose.orientation.w) _, _, yaw = tf.transformations.euler_from_quaternion(quat) ang_err = self.get_goal_angle(yaw, my_pose, self.end[req.pos][:2]) cmd_msg = Twist() if ang_err < -30: cmd_msg.linear.x = -0.15 cmd_msg.angular.z = -0.8 elif ang_err > 30: cmd_msg.linear.x = -0.15 cmd_msg.angular.z = 0.8 else: rospy.loginfo("start plan") break self.pub_cmd.publish(cmd_msg) rospy.sleep(0.1) rospy.sleep(1) # to pre pos -------------------------------------------------------- end_p = PoseStamped() end_p.pose.position.x = self.end[req.pos][0] end_p.pose.position.y = self.end[req.pos][1] end_p.pose.position.z = 0 while not rospy.is_shutdown(): req_path = GetPlanRequest() req_path.start = self.pose req_path.goal = end_p try: res_path = self.req_path_srv(req_path) except: rospy.logerr("%s : path request fail" % rospy.get_name()) res.result = False return res self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(self.pose) if goal is None: break self.pub_marker(goal) self.pub_pid_goal.publish(goal) rospy.sleep(0.1) # to target pos -------------------------------------------------------- # end_p = PoseStamped() # end_p.pose.position.x = self.end[req.pos][0] # end_p.pose.position.y = self.end[req.pos][1] # end_p.pose.position.z = 0 # # pre_p = PoseStamped() # # pre_p.pose.position.x = self.end[req.pos][2] # # pre_p.pose.position.y = self.end[req.pos][3] # # pre_p.pose.position.z = 0 # req_path = GetPlanRequest() # req_path.start = self.pose # req_path.goal = end_p # try: # res_path = self.req_path_srv(req_path) # except: # rospy.logerr("%s : path request fail" % rospy.get_name()) # res.result = False # return res # self.pursuit.set_path(res_path.plan) # while not rospy.is_shutdown(): # goal = self.pursuit.get_goal(self.pose) # if goal is None: # break # self.pub_marker(goal) # self.pub_pid_goal.publish(goal) # rospy.sleep(0.1) res.result = True return res def cb_pose(self, msg): self.pose = msg def get_goal_angle(self, robot_yaw, robot, goal): robot_angle = np.degrees(robot_yaw) p1 = [robot[0], robot[1]] p2 = [robot[0], robot[1]+1.] p3 = goal angle = self.get_angle(p1, p2, p3) result = angle - robot_angle result += 90 return result def get_angle(self, p1, p2, p3): v0 = np.array(p2) - np.array(p1) v1 = np.array(p3) - np.array(p1) angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1)) return np.degrees(angle) # limit the angle to the range of [-180, 180] def angle_range(self, angle): if angle > 180: angle = angle - 360 angle = self.angle_range(angle) elif angle < -180: angle = angle + 360 angle = self.angle_range(angle) return angle
class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = None self.switch_thred = 1 # change to next lane if reach next self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.1) ## node path while not rospy.has_param("/guid_path"): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.last_node = -1 self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False ## set first tracking lane self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_pose", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking) # print("init done") def tracking(self, event): # print("tracking loop") if self.target_global is None: # rospy.logerr("%s : no goal" % rospy.get_name()) return else: rospy.loginfo("%s :have seen goal" % rospy.get_name()) end_p = self.transform_pose(self.target_global.pose, self.bot_frame, self.map_frame) self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1])) d = math.sqrt(end_p.pose.position.x**2 + end_p.pose.position.y**2) if d <= self.switch_thred: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return start_p = PoseStamped() start_p.pose.position.x = 1 start_p.pose.position.y = 0 start_p.pose.position.z = 0 req_path = GetPlanRequest() req_path.start = start_p req_path.goal = end_p try: res_path = self.req_path_srv(req_path) except: rospy.logerr("%s : path request fail" % rospy.get_name()) return self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(start_p) if goal is None: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return goal = self.transform_pose(goal.pose, self.map_frame, self.bot_frame) goal.header.frame_id = self.map_frame self.pub_goal_marker.publish(self.to_marker(goal)) self.pub_pid_goal.publish(goal) def cb_goal(self, msg): self.target_global = msg.pose # posestamped self.bot_frame = self.target_global.header.frame_id self.target_global.pose = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame) self.target_global.header.frame_id = self.map_frame def cb_joy(self, msg): switch = msg.axes[-1] if switch == 0: self.joy_lock = False if self.joy_lock: return switch = msg.axes[-1] if switch == 1: rospy.loginfo("Joy to the next lane.") self.set_lane(True) self.joy_lock = True elif switch == -1: rospy.loginfo("Joy to the last lane.") self.set_lane(False) self.joy_lock = True def set_lane(self, next): # to next node if next: self.last_node += 1 else: self.last_node -= 1 if self.last_node >= len(self.guid_path) - 1: rospy.loginfo("It's the last lane.") self.last_node -= 1 return elif self.last_node < 0: rospy.loginfo("Back to first lane.") self.last_node = 0 # set last, next node (anchor) rospy.set_param("/guid_lane_last", self.all_anchor_ids[self.guid_path[self.last_node]]) rospy.set_param( "/guid_lane_next", self.all_anchor_ids[self.guid_path[self.last_node + 1]]) # set pozyx ranging tag id try: rospy.delete_param("/anchorballs") except KeyError: rospy.logerr("No such param /anchorballs") pozyx_id = {} pozyx_id[self.guid_path[self.last_node]] = self.all_anchor_ids[ self.guid_path[self.last_node]] pozyx_id[self.guid_path[self.last_node + 1]] = self.all_anchor_ids[ self.guid_path[self.last_node + 1]] rospy.set_param("/anchorballs", pozyx_id) def to_marker(self, goal, color=[0, 1, 0]): marker = Marker() marker.header.frame_id = goal.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] return marker def transform_pose(self, pose, target_frame, source_frame): try: (trans_c, rot_c) = self.listener.lookupTransform(target_frame, source_frame, rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("faile to catch tf %s 2 %s" % (target_frame, source_frame)) return trans_mat = tf.transformations.translation_matrix(trans_c) rot_mat = tf.transformations.quaternion_matrix(rot_c) tran_mat = np.dot(trans_mat, rot_mat) # print(trans_c) target_mat = np.array([[1, 0, 0, pose.position.x], [0, 1, 0, pose.position.y], [0, 0, 1, pose.position.z], [0, 0, 0, 1]]) target = np.dot(tran_mat, target_mat) quat = tf.transformations.quaternion_from_matrix(target) trans = tf.transformations.translation_from_matrix(target) t_pose = PoseStamped() t_pose.header.frame_id = target_frame t_pose.pose.position.x = trans[0] t_pose.pose.position.y = trans[1] t_pose.pose.position.z = trans[2] t_pose.pose.orientation.x = quat[0] t_pose.pose.orientation.y = quat[1] t_pose.pose.orientation.z = quat[2] t_pose.pose.orientation.w = quat[3] return t_pose
class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) self.pursuit = PurePursuit() self.pursuit.set_look_ahead_distance(0.2) self.target_global = None self.listener = tf.TransformListener() self.pub_goal_marker = rospy.Publisher( "goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher( "target_marker", Marker, queue_size=1) self.pub_pid_goal = rospy.Publisher( "pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber( "artifact_pose", PoseStamped, self.cb_pose, queue_size=1) self.sub_goal = rospy.Subscriber( "/move_base_simple/goal", PoseStamped, self.cb_goal, queue_size=1) self.timer = rospy.Timer(rospy.Duration(0.2), self.tracking) def to_marker(self, goal, color=[0, 1, 0]): marker = Marker() marker.header.frame_id = goal.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] return marker def transform_pose(self, pose, target_frame, source_frame): try: (trans_c, rot_c) = self.listener.lookupTransform( target_frame, source_frame, rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr("faile to catch tf %s 2 %s" % (target_frame, source_frame)) return trans_mat = tf.transformations.translation_matrix(trans_c) rot_mat = tf.transformations.quaternion_matrix(rot_c) tran_mat = np.dot(trans_mat, rot_mat) target_mat = np.array([[1, 0, 0, pose.position.x], [0, 1, 0, pose.position.y], [0, 0, 1, pose.position.z], [0, 0, 0, 1]]) target = np.dot(tran_mat, target_mat) quat = tf.transformations.quaternion_from_matrix(target) trans = tf.transformations.translation_from_matrix(target) t_pose = PoseStamped() t_pose.header.frame_id = target_frame t_pose.pose.position.x = trans[0] t_pose.pose.position.y = trans[1] t_pose.pose.position.z = trans[2] t_pose.pose.orientation.x = quat[0] t_pose.pose.orientation.y = quat[1] t_pose.pose.orientation.z = quat[2] t_pose.pose.orientation.w = quat[3] return t_pose def tracking(self, event): if self.target_global is None: rospy.logerr("%s : no goal" % rospy.get_name()) return end_p = self.transform_pose( self.target_global.pose, "base_footprint", "map") self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1])) start_p = PoseStamped() start_p.pose.position.x = 1 start_p.pose.position.y = 0 start_p.pose.position.z = 0 req_path = GetPlanRequest() req_path.start = start_p req_path.goal = end_p try: res_path = self.req_path_srv(req_path) except: rospy.logerr("%s : path request fail" % rospy.get_name()) return self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(start_p) if goal is None: rospy.logwarn("goal reached") return goal = self.transform_pose(goal.pose, "map", "base_footprint") self.pub_goal_marker.publish(self.to_marker(goal)) self.pub_pid_goal.publish(goal) def cb_pose(self, msg): for artifact in msg: if artifact.Class == "back_pack": self.target_global = self.transform_pose( artifact.pose, "map", "camera_color_optical_frame") def cb_goal(self, msg): self.target_global = msg
class Navigation(object): def __init__(self): rospy.loginfo("Initializing %s" % rospy.get_name()) ## parameters self.map_frame = rospy.get_param("~map_frame", 'odom') self.bot_frame = 'base_link' self.switch_thred = 1.5 # change to next lane if reach next self.pursuit = PurePursuit() self.lka = rospy.get_param("~lookahead", 0.5) self.pursuit.set_look_ahead_distance(self.lka) ## node path while not rospy.has_param("/guid_path") and not rospy.is_shutdown(): rospy.loginfo("Wait for /guid_path") rospy.sleep(0.5) self.guid_path = rospy.get_param("/guid_path") self.tag_ang_init = rospy.get_param("/guid_path_ang_init") self.last_node = -1 self.next_node_id = None self.all_anchor_ids = rospy.get_param("/all_anchor_ids") self.joy_lock = False self.get_goal = True self.joint_ang = None ## set first tracking lane self.pub_robot_speech = rospy.Publisher("speech_case", String, queue_size=1) self.pub_robot_go = rospy.Publisher("robot_go", Bool, queue_size=1) rospy.sleep(1) # wait for the publisher to be set well self.set_lane(True) # variable self.target_global = None self.listener = tf.TransformListener() # markers self.pub_goal_marker = rospy.Publisher("goal_marker", Marker, queue_size=1) self.pub_target_marker = rospy.Publisher("target_marker", Marker, queue_size=1) # publisher, subscriber self.pub_pid_goal = rospy.Publisher("pid_control/goal", PoseStamped, queue_size=1) self.req_path_srv = rospy.ServiceProxy("plan_service", GetPlan) self.sub_box = rospy.Subscriber("anchor_position", PoseDirectional, self.cb_goal, queue_size=1) sub_joy = rospy.Subscriber('joy_teleop/joy', Joy, self.cb_joy, queue_size=1) sub_fr = rospy.Subscriber('front_right/ranges', DeviceRangeArray, self.cb_range, queue_size=1) sub_joint = rospy.Subscriber('/dynamixel_workbench/joint_states', JointState, self.cb_joint, queue_size=1) #Don't update goal too often self.last_update_goal = None self.goal_update_thred = 0.001 self.hist_goal = np.array([]) self.normal = True self.get_goal = True self.timer = rospy.Timer(rospy.Duration(0.1), self.tracking) # print("init done") # prevent sudden stop self.last_plan = None # keep searching until find path or reach search end self.search_angle = 10 * math.pi / 180 self.search_max = 5 ### stupid range drawing # for using tf to draw range br1 = tf2_ros.StaticTransformBroadcaster() br2 = tf2_ros.StaticTransformBroadcaster() # stf.header.frame_id = "uwb_back_left" # stf.child_frame_id = "base_link" # stf.transform.translation.x = -1*r_tag_points[0, 0] # stf.transform.translation.y = -1*r_tag_points[0, 1] # br1.sendTransform(stf) # stf2 = stf # stf2.header.stamp = rospy.Time.now() # stf2.header.frame_id = "uwb_front_right" # stf2.transform.translation.x = -1*r_tag_points[1, 0] # stf2.transform.translation.y = -1*r_tag_points[1, 1] # br2.sendTransform(stf2) stf = TransformStamped() stf.header.stamp = rospy.Time.now() stf.transform.rotation.w = 1 stf.header.frame_id = "base_link" stf.child_frame_id = "uwb_back_left" stf.transform.translation.x = r_tag_points[0, 0] stf.transform.translation.y = r_tag_points[0, 1] stf2 = TransformStamped() stf2.header.stamp = rospy.Time.now() stf2.transform.rotation.w = 1 stf2.header.frame_id = "base_link" stf2.child_frame_id = "uwb_front_right" stf2.transform.translation.x = r_tag_points[1, 0] stf2.transform.translation.y = r_tag_points[1, 1] # br2.sendTransform([stf, stf2]) # print(rospy.Time.now()) def tracking(self, event): if not self.normal: return st = rospy.Time.now() #rospy.loginfo("tracking loop") if self.target_global is None: rospy.logerr("%s : no goal" % rospy.get_name()) return else: #rospy.loginfo("%s :have seen goal" % rospy.get_name()) pass #print("fuckkckckc why???") #print(self.target_global) end_p = self.transform_pose(self.target_global.pose, self.bot_frame, self.map_frame) #end_p = self.target_global #end_p.header.frame_id = self.map_frame if end_p is None: return end_p.header.frame_id = self.bot_frame start_p = PoseStamped() start_p.pose.position.x = 0 start_p.pose.position.y = 0 start_p.pose.position.z = 0 start_p.header.frame_id = self.bot_frame #start_p = self.transform_pose(start_p.pose, self.map_frame, self.bot_frame) if start_p is None: return req_path = GetPlanRequest() req_path.start = start_p req_path.goal = end_p oe = end_p for i in range(self.search_max): try: res_path = self.req_path_srv(req_path) self.last_plan = res_path #rospy.loginfo("%s : plan success" % rospy.get_name()) break except: rospy.logerr("%s : path request fail" % rospy.get_name()) if self.last_plan is None: return res_path = self.last_plan r = np.linalg.norm([oe.pose.position.x, oe.pose.position.y]) theta = math.atan2(oe.pose.position.y, oe.pose.position.x) theta += (-1)**(i) * (i + 1) * self.search_angle end_p.pose.position.x = r * math.cos(theta) end_p.pose.position.y = r * math.sin(theta) self.pub_target_marker.publish(self.to_marker(end_p, [0, 0, 1])) self.pursuit.set_path(res_path.plan) goal = self.pursuit.get_goal(start_p) if goal is None: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return # print("trace", goal) goal = self.transform_pose(goal.pose, self.map_frame, self.bot_frame) goal.header.frame_id = self.map_frame self.pub_goal_marker.publish(self.to_marker(goal)) self.pub_pid_goal.publish(goal) et = rospy.Time.now() #print("Duration:", et.to_sec()-st.to_sec()) #print("============================================") def cb_goal(self, msg): if not self.get_goal: return self.bot_frame = msg.header.frame_id now_t = rospy.Time.now() if self.last_update_goal is None: self.target_global = msg.pose # posestamped self.target_global = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame_id, msg.header.stamp) if self.target_global is None: return self.target_global.header.frame_id = self.map_frame return dt = now_t.to_sec() - self.last_update_goal.to_sec() if dt >= self.goal_update_thred: tg = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame_id, msg.header.stamp) if tg is None: return tg.header.frame_id = self.map_frame self.hist_goal = np.append(self.hist_goal, tg) self.target_global = tg self.hist_goal = np.array([]) else: tg = self.transform_pose(msg.pose.pose, self.map_frame, msg.header.frame_id, msg.header.stamp) if tg is None: return tg.header.frame_id = self.map_frame self.hist_goal = np.append(self.hist_goal, tg) self.last_update_goal = now_t def cb_range(self, msg): if len(msg.rangeArray) == 0: return d = (msg.rangeArray[0].distance) / 1000. tid = msg.rangeArray[0].tag_id #print("d:", d) #print("tid:",tid) #print("next:",self.next_node_id) if tid != self.next_node_id: return #print("d:",d) #print("self.switch:",self.switch_thred) #print("") if d <= self.switch_thred: rospy.logwarn("goal reached, to next goal") self.target_global = None self.set_lane(True) return #print(self.target_global) def cb_joy(self, msg): #print("cb_joy") #switch = 0 #if msg.axes[-3] < -0.5: # switch = 1 #elif msg.axes[2] < -0.5: # switch = -1 #print("b:",msg.axes[-1]) #print("f:",msg.axes[-2]) switch = None if msg.buttons[-1] == 1 and msg.buttons[-2] == 0: switch = -1 elif msg.buttons[-1] == 0 and msg.buttons[-2] == 1: switch = 1 elif msg.buttons[-1] == 1 and msg.buttons[-2] == 1: return else: switch = 0 # switch = msg.axes[-1] if switch == 0: self.joy_lock = False if self.joy_lock: return if switch == 1: rospy.loginfo("Joy to the next lane.") self.set_lane(True) self.joy_lock = True elif switch == -1: rospy.loginfo("Joy to the last lane.") self.set_lane(False) self.joy_lock = True def cb_joint(self, msg): self.joint_ang = -1 * msg.position[0] def set_lane(self, next): self.pub_robot_go.publish(False) # to next node if next: self.last_node += 1 else: self.last_node -= 1 # select sound file s_anchor = str(self.guid_path[self.last_node]) if len(s_anchor) <= 9: sound_file1 = str(self.guid_path[self.last_node]) + '_1' sound_file2 = str(self.guid_path[self.last_node]) + '_2' else: sound_file1 = str(self.guid_path[self.last_node])[:-2] + '_3' sound_file2 = str(self.guid_path[self.last_node])[:-2] + '_4' ###### head and toe ##################### if self.last_node >= len(self.guid_path) - 1: rospy.loginfo("It's the last lane.") # play sound on robot self.pub_robot_sound(sound_file1) rospy.sleep(2) # this make anchorball speack # see pozyx_ros/src/multi_ranging_guid.py rospy.set_param("/guid_lane_next", 0) rospy.set_param("/velocity_mode", 0) self.last_node -= 1 return elif self.last_node < 0: rospy.loginfo("Back to first lane.") self.last_node = 0 ########################################## # play sound on robot self.pub_robot_sound(sound_file1) rospy.sleep(2) # set pozyx ranging tag id try: rospy.delete_param("/anchorballs") except KeyError: rospy.logerr("No such param /anchorballs") pozyx_id = {} pozyx_id[self.guid_path[self.last_node]] = self.all_anchor_ids[ self.guid_path[self.last_node]] pozyx_id[self.guid_path[self.last_node + 1]] = self.all_anchor_ids[ self.guid_path[self.last_node + 1]] rospy.set_param("/anchorballs", pozyx_id) # set last, next node (anchor) # this make anchorball speack # see pozyx_ros/src/multi_ranging_guid.py # next node id self.target_global = None self.next_node_id = self.all_anchor_ids[self.guid_path[self.last_node + 1]] rospy.set_param("/guid_lane_last", self.all_anchor_ids[self.guid_path[self.last_node]]) rospy.set_param( "/guid_lane_next", self.all_anchor_ids[self.guid_path[self.last_node + 1]]) # start turning (right, left or back) self.normal = False rospy.sleep(2) self.special_case() self.pub_robot_go.publish(True) # play sound on robot self.pub_robot_sound(sound_file2) rospy.sleep(5) # to wait for everything to reset # self.target_global = None def special_case(self): # we hard code turning situation here # not the bast solution, but an achievable one at the moment if self.last_node == 0: self.normal = True return this_ang = self.tag_ang_init[self.guid_path[self.last_node]] if this_ang != 180: self.get_goal = False self.target_global = None while self.target_global is None: self.target_global = PoseStamped() self.target_global.pose.position.x = 5 * math.cos( math.radians(this_ang)) self.target_global.pose.position.y = 5 * math.cos( math.radians(this_ang)) self.target_global = self.transform_pose( self.target_global.pose, self.map_frame, self.bot_frame) self.target_global.header.frame_id = self.map_frame self.normal = True self.pub_robot_go.publish(True) rospy.sleep(5) self.get_goal = True else: self.get_goal = False # 180 degree turn, stage 1, tie up harness while self.joint_ang > math.radians(5): # play sound on robot self.pub_robot_sound('tie_up') rospy.sleep(4) # 180 degree turn, stage 2, turn 180 turn_goal = None print("turn_goal") while turn_goal is None: turn_goal = PoseStamped() turn_goal.pose.position.x = -5 turn_goal_bot = turn_goal turn_goal_pid = turn_goal turn_goal_pid.pose.position.x = -0.05 turn_goal_pid.pose.position.y = -0.005 turn_goal = self.transform_pose(turn_goal.pose, self.map_frame, self.bot_frame) turn_goal_dummy = turn_goal self.pub_robot_go.publish(True) print("after") while math.fabs( math.atan2( turn_goal_bot.pose.position.y, turn_goal_bot.pose.position.x)) >= math.radians(90): print("pub turn goal") self.pub_pid_goal.publish( self.transform_pose(turn_goal_pid.pose, self.map_frame, self.bot_frame)) rospy.sleep(0.1) turn_goal_bot = self.transform_pose(turn_goal.pose, self.bot_frame, self.map_frame) print( math.fabs( math.atan2(turn_goal_bot.pose.position.y, turn_goal_bot.pose.position.x))) self.pub_robot_go.publish(False) # 180 degree turn, stage 3, release harness while self.joint_ang < math.radians(45): # play sound on robot self.pub_robot_sound('release') rospy.sleep(4) self.normal = True self.get_goal = True def pub_robot_sound(self, data): # play sound on robot str_msg = String() str_msg.data = data self.pub_robot_speech.publish(str_msg) def to_marker(self, goal, color=[0, 1, 0]): marker = Marker() marker.header.frame_id = goal.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.orientation.w = 1 marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.id = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] return marker def transform_pose(self, pose, target_frame, source_frame, stamp=None): # print("Test rospy time 0", rospy.Time(0).to_sec()) # print("Test msg time", stamp.to_sec()) #print(target_frame) #print(source_frame) if target_frame is None or source_frame is None: return try: (trans_c, rot_c) = self.listener.lookupTransform(target_frame, source_frame, rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo("faile to catch tf %s 2 %s" % (target_frame, source_frame)) return trans_mat = tf.transformations.translation_matrix(trans_c) rot_mat = tf.transformations.quaternion_matrix(rot_c) tran_mat = np.dot(trans_mat, rot_mat) # print(trans_c) target_mat = np.array([[1, 0, 0, pose.position.x], [0, 1, 0, pose.position.y], [0, 0, 1, pose.position.z], [0, 0, 0, 1]]) target = np.dot(tran_mat, target_mat) quat = tf.transformations.quaternion_from_matrix(target) trans = tf.transformations.translation_from_matrix(target) t_pose = PoseStamped() t_pose.header.frame_id = target_frame t_pose.pose.position.x = trans[0] t_pose.pose.position.y = trans[1] t_pose.pose.position.z = trans[2] t_pose.pose.orientation.x = quat[0] t_pose.pose.orientation.y = quat[1] t_pose.pose.orientation.z = quat[2] t_pose.pose.orientation.w = quat[3] return t_pose