def get_command(self, cur, goal, dT):
"""
cur: ChassisState
goal: ChassisState
dT: float
"""
desired = ChassisState()
t_err = cur.t - goal.t
x_err = cur.x - goal.y
y_err = cur.y - goal.y
# From section 5.12, https://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf
desired.vx = (goal.vx * math.cos(t_err) +
self.gain_fn(goal.vx, goal.vt, self.b, self.zeta) *
(x_err * math.cos(goal.t) + y_err * math.sin(goal.t)))
desired.vt = (
goal.vt + (self.b * goal.vx * math.sin(t_err) / t_err) *
(y_err * math.cos(goal.t) - x_err * math.sin(goal.t)) +
self.gain_fn(goal.vx, goal.vt, self.b, self.zeta) * t_err)
return desired
def __init__(self):
"""
Initializes ROS and necessary services and publishers.
"""
rospy.init_node("chassis_planning"
# disable_signals=True
# log_level=rospy.DEBUG
)
rospy.on_shutdown(self.shutdown_hook)
self.base_max_speed = rospy.get_param("~base_max_speed",
0.36) # m/s, absolute max: 0.36
self.base_max_ang_v = rospy.get_param("~base_max_ang_v",
0.7) # rad/s, absolute max: 7.0
self.kP = rospy.get_param("~kP", 3.0)
self.kA = rospy.get_param("~kA", 8.0)
self.kB = rospy.get_param("~kB", -1.5)
self.twist_command = geometry_msgs.msg.Twist()
self.cmd_vel_pub = rospy.Publisher("cmd_vel",
geometry_msgs.msg.Twist,
queue_size=100)
self.result = ChassisResult()
self.pos_tolerance = 0.01 # m
self.angle_tolerance = 0.05 # rad
self.base_speed = self.base_max_speed
self.base_ang_v = self.base_max_ang_v
self.min_ang_v = 0.1 # rad/s, cold start minimum: 0.75
self.min_speed = 0.01 # m/s
self.base_link_frame = "base_link"
self.map_frame = "map"
self.chassis_action_name = rospy.get_param("~chassis_action_name",
"chassis_actions")
self.chassis_action_name = rospy.get_param("~chassis_action_name",
"chassis_actions")
self.chassis_server = actionlib.SimpleActionServer(
self.chassis_action_name,
ChassisAction,
self.chassis_callback,
auto_start=False)
self.chassis_server.start()
rospy.loginfo("[%s] server started" % self.chassis_action_name)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.state = ChassisState()
self.controller = GoalController()
self.controller.set_constants(self.kP, self.kA, self.kB)
# self.controller.set_constants(3.5, 20.0, -1.0)
self.controller.forward_movement_only = False
self.chassis_ready_service_name = "chassis_ready_service"
rospy.loginfo("Setting up service %s" %
self.chassis_ready_service_name)
self.chassis_ready_srv = rospy.Service(self.chassis_ready_service_name,
Trigger,
self.chassis_ready_callback)
rospy.loginfo("%s service is ready" % self.chassis_ready_service_name)
rospy.loginfo("[%s] --- Dodobot chassis planning is up! ---" %
self.chassis_action_name)
def goto_pose(self, params):
default_val = None # float("nan")
goal_x = params.get("goal_x", default_val)
goal_y = params.get("goal_y", default_val)
goal_angle = params.get("goal_angle", default_val)
self.base_speed = params.get("base_speed", self.base_speed)
self.base_ang_v = params.get("base_ang_v", self.base_ang_v)
self.pos_tolerance = params.get("pos_tolerance", self.pos_tolerance)
self.angle_tolerance = params.get("angle_tolerance",
self.angle_tolerance)
drive_forwards = bool(params.get("drive_forwards"))
goal_state = ChassisState(goal_x, goal_y, goal_angle)
rospy.loginfo("Current pose: %s. Goal pose: %s" %
(self.state, goal_state))
self.controller.max_linear_speed = self.base_speed
self.controller.min_linear_speed = self.min_speed
self.controller.max_angular_speed = self.base_ang_v
self.controller.min_angular_speed = self.min_ang_v
self.controller.linear_tolerance = self.pos_tolerance
self.controller.angular_tolerance = self.angle_tolerance
self.controller.reverse_direction = not drive_forwards
rate = rospy.Rate(60.0)
prev_time = rospy.Time.now()
was_at_goal = False
am_at_goal_timeout = rospy.Duration(0.25)
am_at_goal_timer = rospy.Time.now()
while True:
rate.sleep()
self.update_current_pose()
rospy.loginfo("goal: %s, current: %s, error: %s" %
(goal_state, self.state, goal_state - self.state))
is_at_goal = self.controller.at_goal(self.state, goal_state)
if is_at_goal:
break
# if is_at_goal and abs(command_state.vx) < 0.001 and abs(command_state.vt) < 0.001:
# break
# if is_at_goal:
# if is_at_goal != was_at_goal:
# am_at_goal_timer = rospy.Time.now()
# was_at_goal = is_at_goal
# self.send_stop()
# if rospy.Time.now() - am_at_goal_timer > am_at_goal_timeout:
# break
# else:
# was_at_goal = is_at_goal
current_time = rospy.Time.now()
dt = (current_time - prev_time).to_sec()
prev_time = current_time
command_state = self.controller.get_command(
self.state, goal_state, dt)
self.send_cmd(command_state.vx, command_state.vt)
if self.controller.is_stuck():
rospy.logwarn("Robot got stuck during go to pose routine!")
rospy.loginfo(
"goal: %s, current: %s, error: %s" %
(goal_state, self.state, goal_state - self.state))
return False
if self.check_cancelled():
rospy.loginfo("Go to pose routine cancelled")
return False
rospy.loginfo("goto_pose command completed successfully!")
rospy.loginfo("goal: %s, current: %s, error: %s" %
(goal_state, self.state, goal_state - self.state))
return True
def get_velocity(self, cur, goal, dT):
"""
cur: ChassisState
goal: ChassisState
dT: float
"""
desired = ChassisState()
goal_heading = math.atan2(goal.y - cur.y, goal.x - cur.x)
if self.reverse_direction:
goal_heading = self.normalize_pi(goal_heading + math.pi)
a = -cur.t + goal_heading
if goal.t is None:
goal_t = goal_heading
else:
goal_t = goal.t
# In Automomous Mobile Robots, they assume theta_G=0. So for
# the error in heading, we have to adjust theta based on the
# (possibly non-zero) goal theta.
theta = self.normalize_pi(cur.t - goal_t)
b = -theta - a
# rospy.loginfo('cur=%f goal=%f a=%f b=%f', cur.theta, goal_heading,
# a, b)
d = self.get_goal_distance(cur, goal)
if self.forward_movement_only:
direction = 1.0
a = self.normalize_pi(a)
b = self.normalize_pi(b)
else:
direction = math.copysign(1.0, math.cos(a))
a = self.normalize_half_pi(a)
b = self.normalize_half_pi(b)
if self.reverse_direction:
direction *= -1.0
# rospy.loginfo('After normalization, a=%f b=%f', a, b)
if abs(d) < self.linear_tolerance:
desired.vx = 0.0
desired.vt = self.kB * theta
else:
desired.vx = self.kP * d * direction
desired.vt = self.kA * a + self.kB * b
# TBD: Adjust velocities if linear or angular acceleration
# too high.
# Adjust velocities if X velocity is too high.
if abs(desired.vx) > self.max_linear_speed:
ratio = self.max_linear_speed / abs(desired.vx)
desired.vx *= ratio
desired.vt *= ratio
# Adjust velocities if turning velocity too high.
if abs(desired.vt) > self.max_angular_speed:
ratio = self.max_angular_speed / abs(desired.vt)
desired.vx *= ratio
desired.vt *= ratio
# Adjust velocities if too low, so robot does not stall.
if abs(desired.vx) > 0 and abs(desired.vx) < self.min_linear_speed:
ratio = self.min_linear_speed / abs(desired.vx)
desired.vx *= ratio
desired.vt *= ratio
elif desired.vx == 0.0 and desired.vt != 0.0 and abs(
desired.vt) < self.min_angular_speed:
ratio = self.min_angular_speed / abs(desired.vt)
desired.vx *= ratio
desired.vt *= ratio
return desired
def get_command(self, current_state, goal_state, dT):
return ChassisState()