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
