def __init__(self): self._name = rospy.get_name() self.last_time = rospy.Time.now() self.rate_hz = 200 self.rate = rospy.Rate(self.rate_hz) self.get_params() self.state = BicycleStateMsg() self.command = BicycleCommandMsg() if self.turtlesim: self.turtlesim_subscriber = rospy.Subscriber(self.turtlesim_pose_topic, Pose, self.update_turtlesim_pose) self.unicycle_command_topic = self.turtlesim_command_topic # resetting stuff print 'Waiting for turtlesim/reset service ...', rospy.wait_for_service('reset') print 'found!' self.reset_turtlesim_service = rospy.ServiceProxy('reset', EmptySrv) else: self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.unicycle_command_topic = self.turtlebot_command_topic # resetting stuff self.reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry', EmptyMsg, queue_size=10) self.command_publisher = rospy.Publisher(self.unicycle_command_topic, Twist, queue_size = 1) self.state_publisher = rospy.Publisher(self.state_topic, BicycleStateMsg, queue_size = 1) self.subscriber = rospy.Subscriber(self.bicycle_command_topic, BicycleCommandMsg, self.command_listener) rospy.Service('converter/reset', EmptySrv, self.reset) rospy.on_shutdown(self.shutdown)
def reset(self, req): if self.turtlesim: self.reset_turtlesim_service() else: self.reset_odom.publish(EmptyMsg()) self.state = BicycleStateMsg() return EmptyResponse()
def reset(self, req): if self.turtlesim: self.reset_turtlesim_service() else: print 'RESETTING TURTLEBOT ODOMETRY' self.reset_odom.publish(EmptyMsg()) self.state = BicycleStateMsg() return EmptyResponse()
def __init__(self): self.pub = rospy.Publisher('/bicycle/cmd_vel', BicycleCommandMsg, queue_size=10) self.sub = rospy.Subscriber('/bicycle/state', BicycleStateMsg, self.subscribe) self.rate = rospy.Rate(200) self.state = BicycleStateMsg()
def __init__(self): """ Executes a plan made by the planner """ self.pub = rospy.Publisher('/bicycle/cmd_vel', BicycleCommandMsg, queue_size=10) self.sub = rospy.Subscriber('/bicycle/state', BicycleStateMsg, self.subscribe ) self.rate = rospy.Rate(100) self.state = BicycleStateMsg()
def __init__(self): """ Executes a plan made by the planner """ self.pub = rospy.Publisher('/bicycle/cmd_vel', BicycleCommandMsg, queue_size=10) self.sub = rospy.Subscriber('/bicycle/state', BicycleStateMsg, self.subscribe ) self.rate = rospy.Rate(100) self.state = BicycleStateMsg() rospy.on_shutdown(self.shutdown) self.start_time = None self.times = [] self.plan_states = [] self.actual_states = []
def __init__(self): """ Executes a plan made by the planner """ self.pub = rospy.Publisher('/bicycle/cmd_vel', BicycleCommandMsg, queue_size=10) self.sub = rospy.Subscriber('/bicycle/state', BicycleStateMsg, self.subscribe) self.rate = rospy.Rate(100) self.state = BicycleStateMsg() self.state_np = np.ones(4) * float('inf') self.states = [] rospy.on_shutdown(self.shutdown)
def v_path_to_u_path_singular(self, path, start_state, dt): def v2cmd(v1, v2, state): u1 = v1 u2 = v2 return BicycleCommandMsg(u1, u2) curr_state = copy(start_state) for i, (t, v1, v2) in enumerate(path): cmd_u = v2cmd(v1, v2, curr_state) path[i] = [t, cmd_u, curr_state] curr_state = BicycleStateMsg( curr_state.x + np.cos(curr_state.theta) * cmd_u.linear_velocity * dt, curr_state.y + np.sin(curr_state.theta) * cmd_u.linear_velocity * dt, curr_state.theta + np.tan(curr_state.phi) / float(self.l) * cmd_u.linear_velocity * dt, curr_state.phi + cmd_u.steering_rate * dt) return path
def cmd(self, u1, u2, dt): """ BicycleCommandMsg: The commands to a bicycle model robot (v, phi) float64 linear_velocity float64 steering_rate """ # self.path.append((self.t, u1, u2)) curr_state = self.state # for i, (self.t, u1, u2) in enumerate(path): cmd_u = BicycleCommandMsg(u1, u2) self.path.append([self.t, cmd_u, curr_state]) print([self.t, cmd_u, curr_state]) self.state = BicycleStateMsg( curr_state.x + np.cos(curr_state.theta) * cmd_u.linear_velocity*dt, curr_state.y + np.sin(curr_state.theta) * cmd_u.linear_velocity*dt, curr_state.theta + np.tan(curr_state.phi) / float(self.l) * cmd_u.linear_velocity*dt, curr_state.phi + cmd_u.steering_rate*dt ) # print(self.state) self.t += dt
def v_path_to_u_path(self, path, start_state, dt): """ convert a trajectory in v commands to u commands Parameters ---------- path : :obj:`list` of (float, float, float) list of (time, v1, v2) commands start_state : :obj:`BicycleStateMsg` starting state of this trajectory dt : float how many seconds between timesteps in the trajectory Returns ------- :obj:`list` of (time, BicycleCommandMsg, BicycleStateMsg) This is a list of timesteps, the command to be sent at that time, and the predicted state at that time """ def v2cmd(v1, v2, state): u1 = v1 / np.cos(state.theta) u2 = v2 return BicycleCommandMsg(u1, u2) curr_state = copy(start_state) for i, (t, v1, v2) in enumerate(path): cmd_u = v2cmd(v1, v2, curr_state) path[i] = [t, cmd_u, curr_state] curr_state = BicycleStateMsg( curr_state.x + np.cos(curr_state.theta) * cmd_u.linear_velocity * dt, curr_state.y + np.sin(curr_state.theta) * cmd_u.linear_velocity * dt, curr_state.theta + np.tan(curr_state.phi) / float(self.l) * cmd_u.linear_velocity * dt, curr_state.phi + cmd_u.steering_rate * dt) return path
args = parse_args() # reset turtlesim state print 'Waiting for converter/reset service ...', rospy.wait_for_service('/converter/reset') print 'found!' reset = rospy.ServiceProxy('/converter/reset', EmptySrv) reset() ex = Exectutor() print "Initial State" print ex.state p = SinusoidPlanner(l, 0.3, 2, 3) goalState = BicycleStateMsg(args.x, args.y, args.theta, args.phi) if (args.theta > 1): plan = p.plan_to_pose3(ex.state, goalState, 0.01, 6) else: plan = p.plan_to_pose(ex.state, goalState, 0.01, 6) print "Predicted Initial State" print plan[0][2] print "Predicted Final State" print plan[-1][2] # gains = ex.calculateGain(plan) # ex.executeGain(plan, gains) ex.execute(plan) print "Final State" print ex.state
# reset turtlesim state print 'Waiting for converter/reset service ...', rospy.wait_for_service('/converter/reset') print 'found!' reset = rospy.ServiceProxy('/converter/reset', EmptySrv) reset() ex = Exectutor() print "Initial State" print ex.state l = 0.3 p = SinusoidPlanner(l, 0.3, 2, 3) goalState = BicycleStateMsg(args.x, args.y, args.theta, args.phi) delta_t = 10 ''' plan = p.plan_to_pose(ex.state, goalState, 0.01, delta_t) ''' full_turn_angle = np.pi/4 plan = [] if args.theta > full_turn_angle: num_full_turns = int(args.theta / full_turn_angle) remainder_turn_angle = args.theta % full_turn_angle for turn_nbr in range(num_full_turns): temp_goalState = BicycleStateMsg(args.x, args.y, (turn_nbr+1)*full_turn_angle, args.phi) temp_plan = p.plan_to_pose(ex.state, temp_goalState, 0.01, delta_t)
def plan_to_pose(self, start_state, goal_state, dt = 0.01, delta_t=2): """ Plans to a specific pose in (x,y,theta,phi) coordinates. You may or may not have to convert the state to a v state with state2v() This is a very optional function. You may want to plan each component separately so that you can reset phi in case there's drift in phi Parameters ---------- start_state: :obj:`BicycleStateMsg` goal_state: :obj:`BicycleStateMsg` dt : float how many seconds between trajectory timesteps delta_t : float how many seconds each trajectory segment should run for Returns ------- :obj:`list` of (float, BicycleCommandMsg, BicycleStateMsg) This is a list of timesteps, the command to be sent at that time, and the predicted state at that time """ # copy goal state goal_state = BicycleStateMsg(goal_state.x, goal_state.y, goal_state.theta, goal_state.phi) # This bit hasn't been exhaustively tested, so you might hit a singularity anyways max_abs_angle = max(abs(goal_state.theta), abs(start_state.theta)) min_abs_angle = min(abs(goal_state.theta), abs(start_state.theta)) if (max_abs_angle > np.pi/2) and (min_abs_angle < np.pi/2): # raise ValueError("You'll cause a singularity here. You should add something to this function to fix it") print 'SINGULARITY DETECTED' theta_dir = np.sign(goal_state.theta - start_state.theta) # which direction theta is changing # Use alternate model if not near 0 or pi if not(abs(start_state.theta) < 0.1 or abs(start_state.theta - np.pi) < 0.1): print 'USING ALTERNATE MODEL.' # Move goal theta so it's not near a singularity if abs(goal_state.theta) < 0.1 or abs(goal_state.theta - np.pi) < 0.1: old_goal_theta = goal_state.theta goal_state.theta -= theta_dir * 0.2 print 'ADJUSTED GOAL THETA FROM {0} TO {1} SO IT IS NOT AT SINGULARITY.'.format(old_goal_theta, goal_state.theta) return self.alternate_sinusoid_planner.plan_to_pose(start_state, goal_state, dt=dt, delta_t=delta_t) else: # Move away from the alternate model's singularity using this model goal_state.theta = start_state.theta + theta_dir * 0.2 print 'USING REGULAR MODEL TO MOVE TO THETA = {0}'.format(goal_state.theta) if abs(start_state.phi) > self.max_phi or abs(goal_state.phi) > self.max_phi: raise ValueError("Either your start state or goal state exceeds steering angle bounds") # We can only change phi up to some threshold self.phi_dist = min( abs(goal_state.phi - self.max_phi), abs(goal_state.phi + self.max_phi) ) x_path = self.steer_x( start_state, goal_state, 0, dt, delta_t ) phi_path = self.steer_phi( x_path[-1][2], goal_state, x_path[-1][0] + dt, dt, delta_t ) alpha_path = self.steer_alpha( phi_path[-1][2], goal_state, phi_path[-1][0] + dt, dt, delta_t ) y_path = self.steer_y( alpha_path[-1][2], goal_state, alpha_path[-1][0] + dt, dt, delta_t ) path = [] for p in [x_path, phi_path, alpha_path, y_path]: path.extend(p) return path