def cbWheelsCmd(self, msg):
trimmed_cmd = WheelsCmd()
trimmed_cmd.vel_left = np.clip(msg.vel_left * (1.0 - self.trim), -1.0,
1.0)
trimmed_cmd.vel_right = np.clip(msg.vel_right * (1.0 + self.trim),
-1.0, 1.0)
self.pub_topic.publish(trimmed_cmd)
def publishControl(self):
speed = self.joy.axes[1] * self.speed_gain #Left stick V-axis. Up is positive
steering = self.joy.axes[3] * self.steer_gain
wheels_cmd_msg = WheelsCmd()
# Car Steering Mode
vel_left = (speed - steering)
vel_right = (speed + steering)
wheels_cmd_msg.vel_left = np.clip(vel_left,-1.0,1.0)
wheels_cmd_msg.vel_right = np.clip(vel_right,-1.0,1.0)
rospy.loginfo("[%s] left %f, right %f" % (self.node_name,wheels_cmd_msg.vel_left,wheels_cmd_msg.vel_right))
self.pub_wheels.publish(wheels_cmd_msg)
def publishControl(self):
speed = self.joy.axes[
1] * self.speed_gain #Left stick V-axis. Up is positive
steering = self.joy.axes[3] * self.steer_gain
wheels_cmd_msg = WheelsCmd()
# Car Steering Mode
vel_left = (speed - steering)
vel_right = (speed + steering)
wheels_cmd_msg.vel_left = np.clip(vel_left, -1.0, 1.0)
wheels_cmd_msg.vel_right = np.clip(vel_right, -1.0, 1.0)
rospy.loginfo("[%s] left %f, right %f" %
(self.node_name, wheels_cmd_msg.vel_left,
wheels_cmd_msg.vel_right))
self.pub_wheels.publish(wheels_cmd_msg)
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initialzing." %(self.node_name))
# Setup publishers
self.pub_topic_a = rospy.Publisher("~topic_a",String, queue_size=1)
self.pub_wheels_cmd = rospy.Publisher("/kitt/wheels_driver/wheels_cmd",WheelsCmd, queue_size=1)
# Setup subscriber
self.sub_topic_b = rospy.Subscriber("~topic_b", String, self.cbTopic)
# Read parameters
self.pub_timestep = self.setupParameter("~pub_timestep",1.0)
# Create a timer that calls the cbTimer function every 1.0 second
self.timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),self.cbTimer)
rospy.loginfo("[%s] Initialzed." %(self.node_name))
rate = rospy.Rate(30) # 10hz
#move forward
forward_for_time_leave = 2.25
turn_for_time = 0.75
forward_for_time_enter = 1
starting_time = rospy.Time.now()
while((rospy.Time.now() - starting_time) < rospy.Duration(forward_for_time_leave)):
wheels_cmd_msg = WheelsCmd()
wheels_cmd_msg.vel_left = 0.4
wheels_cmd_msg.vel_right = 0.4
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
rate.sleep()
#turn right
starting_time = rospy.Time.now()
while((rospy.Time.now() - starting_time) < rospy.Duration(turn_for_time)):
wheels_cmd_msg = WheelsCmd()
wheels_cmd_msg.vel_left = 0.5
wheels_cmd_msg.vel_right = -0.5
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
rate.sleep()
#move forward
starting_time = rospy.Time.now()
while((rospy.Time.now() - starting_time) < rospy.Duration(forward_for_time_enter)):
wheels_cmd_msg = WheelsCmd()
wheels_cmd_msg.vel_left = 0.4
wheels_cmd_msg.vel_right = 0.4
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
rate.sleep()
def publishCmd(self,stamp,speed,steering):
lane_cmd_msg = CarControl()
lane_cmd_msg.header.stamp = stamp
lane_cmd_msg.speed = speed
lane_cmd_msg.steering = steering
lane_cmd_msg.need_steering = True
wheels_cmd_msg = WheelsCmd()
# wheels_cmd_msg.header.stamp = stamp
ratio = 1.0
gain = 0.5
vel_left = gain*(speed - steering)*ratio
vel_right = gain*(speed + steering)*(1.0/ratio)
wheels_cmd_msg.vel_left = np.clip(vel_left,-1.0,1.0)
wheels_cmd_msg.vel_right = np.clip(vel_right,-1.0,1.0)
self.pub_lane_cmd.publish(lane_cmd_msg)
self.pub_wheels_cmd.publish(wheels_cmd_msg)
def publishCmd(self, stamp, speed, steering):
lane_cmd_msg = CarControl()
lane_cmd_msg.header.stamp = stamp
lane_cmd_msg.speed = speed
lane_cmd_msg.steering = steering
lane_cmd_msg.need_steering = True
wheels_cmd_msg = WheelsCmd()
# wheels_cmd_msg.header.stamp = stamp
ratio = 1.0
gain = 0.5
vel_left = gain * (speed - steering) * ratio
vel_right = gain * (speed + steering) * (1.0 / ratio)
wheels_cmd_msg.vel_left = np.clip(vel_left, -1.0, 1.0)
wheels_cmd_msg.vel_right = np.clip(vel_right, -1.0, 1.0)
self.pub_lane_cmd.publish(lane_cmd_msg)
self.pub_wheels_cmd.publish(wheels_cmd_msg)
def cbWheelsCmd(self,msg):
trimmed_cmd = WheelsCmd()
trimmed_cmd.vel_left = np.clip(msg.vel_left*(1.0-self.trim),-1.0,1.0)
trimmed_cmd.vel_right = np.clip(msg.vel_right*(1.0+self.trim),-1.0,1.0)
self.pub_topic.publish(trimmed_cmd)
def __init__(self):
# Save the name of the node
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initialzing." % (self.node_name))
# Setup publishers
self.pub_topic_a = rospy.Publisher("~topic_a", String, queue_size=1)
self.pub_wheels_cmd = rospy.Publisher("/kitt/wheels_driver/wheels_cmd",
WheelsCmd,
queue_size=1)
# Setup subscriber
self.sub_topic_b = rospy.Subscriber("~topic_b", String, self.cbTopic)
# Read parameters
self.pub_timestep = self.setupParameter("~pub_timestep", 1.0)
# Create a timer that calls the cbTimer function every 1.0 second
self.timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),
self.cbTimer)
rospy.loginfo("[%s] Initialzed." % (self.node_name))
rate = rospy.Rate(30) # 10hz
#move forward
forward_for_time_leave = 2.25
turn_for_time = 0.75
forward_for_time_enter = 1
starting_time = rospy.Time.now()
while ((rospy.Time.now() - starting_time) <
rospy.Duration(forward_for_time_leave)):
wheels_cmd_msg = WheelsCmd()
wheels_cmd_msg.vel_left = 0.4
wheels_cmd_msg.vel_right = 0.4
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
rate.sleep()
#turn right
starting_time = rospy.Time.now()
while ((rospy.Time.now() - starting_time) <
rospy.Duration(turn_for_time)):
wheels_cmd_msg = WheelsCmd()
wheels_cmd_msg.vel_left = 0.5
wheels_cmd_msg.vel_right = -0.5
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
rate.sleep()
#move forward
starting_time = rospy.Time.now()
while ((rospy.Time.now() - starting_time) <
rospy.Duration(forward_for_time_enter)):
wheels_cmd_msg = WheelsCmd()
wheels_cmd_msg.vel_left = 0.4
wheels_cmd_msg.vel_right = 0.4
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
rate.sleep()
