def __init__(self):
# Get parameters
self.cmd_vel_sub = rospy.get_param("~cmd_vel_sub",
"/fmSignals/cmd_vel_sp")
self.cmd_vel_pub = rospy.get_param("~cmd_vel_sub",
"/fmSignals/cmd_vel")
self.frame_id = rospy.get_param("~frame_id", 'base_link')
self.world_frame = rospy.get_param("~world_frame", 'map')
self.rate = rospy.Rate(50.0)
# Init node
self.twist_sub = rospy.Subscriber(self.cmd_vel_sub, TwistStamped,
self.onTwist)
self.twist_pub = rospy.Publisher(self.cmd_vel_pub, TwistStamped)
self.twist = Twist()
self.tf = TransformListener()
self.controller = Controller()
def __init__(self):
# Init line
self.rabbit_factor = 0.2
self.line_begin = Vector(0, 0)
self.line_end = Vector(0, 5)
self.line = Vector(self.line_end[0] - self.line_begin[0],
self.line_end[1] - self.line_begin[1])
self.yaw = 0.0
# Init control methods
self.isNewGoalAvailable = self.empty_method()
self.isPreemptRequested = self.empty_method()
self.setPreempted = self.empty_method()
# Init velocity control
self.controller = Controller()
self.distance_to_goal = 0
self.angle_error = 0
self.goal_angle_error = 0
self.sp_linear = 0
self.sp_angular = 0
self.distance_to_line = 0
# Get parameters from parameter server
self.getParameters()
# Set up markers for rviz
self.point_marker = MarkerUtility("/point_marker", self.odom_frame)
self.line_marker = MarkerUtility("/line_marker", self.odom_frame)
self.pos_marker = MarkerUtility("/pos_marker", self.odom_frame)
# Init TF listener
self.__listen = TransformListener()
# Init planner
self.corrected = False
self.rate = rospy.Rate(1 / self.period)
self.twist = TwistStamped()
self.quaternion = np.empty((4, ), dtype=np.float64)
# Init vectors
self.destination = Vector(1, 0)
self.position = Vector(
1, 0) # Vector from origo to current position of the robot
self.heading = Vector(
1, 0) # Vector in the current direction of the robot
self.rabbit = Vector(
1, 0) # Vector from origo to the aiming point on the line
self.rabbit_path = Vector(
1, 0) # Vector from current position to aiming point on the line
self.perpendicular = Vector(
1, 0
) # Vector from current position to the line perpendicular to the line
self.projection = Vector(
1, 0) # Projection of the position vector on the line
# Set up circular buffer for filter
self.zone_filter = [self.z3_value] * int(self.z_filter_size)
self.zone = 2
self.z_ptr = 0
# Setup Publishers and subscribers
if not self.use_tf:
self.odom_sub = rospy.Subscriber(self.odometry_topic, Odometry,
self.onOdometry)
self.twist_pub = rospy.Publisher(self.cmd_vel_topic, TwistStamped)
# Setup dynamic reconfigure
if self.use_dynamic_reconfigure:
self.reconfigure_server = Server(ParametersConfig,
self.reconfigure_cb)
self.twist = Twist()
self.tf = TransformListener()
self.controller = Controller()
def onTwist(self, msg):
self.updateFeedback()
self.twist = self.controller.generateTwist(msg.twist.linear.x,
msg.twist.angular.z)
self.twist_pub.publish(self.twist)
def updateFeedback(self):
try:
(position,
heading) = self.__listen.lookupTransform(self.frame_id,
self.world_frame,
rospy.Time(0))
(roll, pitch,
yaw) = tf.transformations.euler_from_quaternion(heading)
self.controller.setFeedback(Vector(position[0], position[1]),
Vector(math.cos(yaw), math.sin(yaw)))
except (tf.LookupException, tf.ConnectivityException), err:
rospy.loginfo("could not locate vehicle")
if __name__ == '__main__':
rospy.init_node('cmd_vel_controller')
node = Controller()
reconfigure_server = Server(ParametersConfig, node.reconfigure_cb)
rospy.spin()
