def listener():
# =============================================================================
# # Launch a node called "mobile_robot_animator"
# =============================================================================
rospy.init_node('mobile_robot_animator', anonymous=False)
# =============================================================================
# #Create a mobile robot object from the Imported module "me439_mobile_robot_class"
# =============================================================================
robot = m439rbt.robot(wheel_width, body_length, wheel_radius)
sub_robot_pose = rospy.Subscriber('/robot_pose_simulated', Pose2D, set_robot_pose, robot)
# Call the animation function. This has a loop in it, so it won't exit.
animate_robot(robot)
# # NEW: Determine paths by lines, arcs, pivots and pauses, and create a
# # "robot" object to plan it for you.
# =============================================================================
# Get parameters from rosparam
wheel_width = rospy.get_param(
'/wheel_width_model'
) # All you have when planning is a model - you never quite know the truth!
body_length = rospy.get_param('/body_length')
wheel_diameter = rospy.get_param('/wheel_diameter_model')
wheel_radius = wheel_diameter / 2.0
# Create a mobile robot object from the Imported module "me439_mobile_robot_class"
# REMEMBER to call the right file (i.e., use the _HWK if needed)
import me439_mobile_robot_class_v01 as m439rbt
robot = m439rbt.robot(wheel_width, body_length, wheel_radius)
#%%
###############################################################################
# PATH_SPECS for a Closed-Loop path-following controller.
# Each segment is defined as an Arc with parameters:
# (Xorigin, Yorigin, Theta_init, Rsigned, Length)
#- Xorigin and Yorigin are in the World frame
#- Theta_init is in the World frame, measuring the angle of the intended ROBOT FORWARD
# direction as a +z rotation from the +Yworld axis
#- Rsigned should be + for Left turn, - for Right turn, or +/- infinity (np.inf (numpy.inf)) for a Line
#- Length is a path Arclength for this segment. (for a Line, the line's length)
###############################################################################
# SEVERAL EXAMPLES of Direct path programming:
# the last one active will be executed.
###############################################################################