def rotate(self, velocity, time):
print "Rotate"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = -velocity
self.pub.publish(msg)
sleep(time)
print "Rotate:done"
def on_shutdown(self):
rospy.loginfo("[%s] Shutting down." % (self.node_name))
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
wcs = WheelsCmdStamped()
wcs.header = h
wcs.vel_left = 0
wcs.vel_right = 0
self.pub_wheels_cmd.publish(wcs)
def custom_shutdown(self):
stop_msg = WheelsCmdStamped()
stop_msg.header.stamp = rospy.get_rostime()
stop_msg.vel_left = 0.0
stop_msg.vel_right = 0.0
self.pub_wheels_cmd.publish(stop_msg)
rospy.sleep(0.5)
rospy.loginfo("Shutdown complete oder?")
def stop(self):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = 0.0
msg.vel_right = 0.0
self.wheelpub.publish(msg)
message = "Stop"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def left_turn(self, time):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = self.speed
msg.vel_right = self.speed + 0.3
self.wheelpub.publish(msg)
self.set_leds("left_turn")
rospy.sleep(time)
def forward(self, velocity, time):
print "Forward"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = velocity
self.pub.publish(msg)
sleep(time)
print "distance:'%s'" % (velocity * time)
def carCmdCallback(self, msg_car_cmd):
[d_L, d_R] = self.ik.evaluate(msg_car_cmd.omega, msg_car_cmd.v)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header = msg_car_cmd.header
msg_wheels_cmd.vel_left = d_L
msg_wheels_cmd.vel_right = d_R
self.pub_wheels_cmd.publish(msg_wheels_cmd)
def forward(self, time):
# print "Forward"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = self.speed
msg.vel_right = self.speed
self.wheelpub.publish(msg)
self.set_leds("forward")
rospy.sleep(time)
def execute(self, inputs, outputs, gvm):
drive_cmd = WheelsCmdStamped()
node = gvm.get_variable("Node", True)
wheels_cmd_pub = node.create_publisher(WheelsCmdStamped, '/None/wheels_driver_node/wheels_cmd', 10)
drive_cmd.vel_right = 0.8
drive_cmd.vel_left = 0.8
wheels_cmd_pub.publish(drive_cmd)
return "success"
def turnRight(self):
#move forward
forward_for_time_leave = 2.0
turn_for_time = 0.7
forward_for_time_enter = 2.0
starting_time = rospy.Time.now()
while ((rospy.Time.now() - starting_time) <
rospy.Duration(forward_for_time_leave)):
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
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?.")
self.rate.sleep()
#turn right
starting_time = rospy.Time.now()
while ((rospy.Time.now() - starting_time) <
rospy.Duration(turn_for_time)):
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
wheels_cmd_msg.vel_left = 0.25
wheels_cmd_msg.vel_right = -0.25
self.pub_wheels_cmd.publish(wheels_cmd_msg)
rospy.loginfo("Moving?.")
self.rate.sleep()
#coordination with lane controller means part way through announce finished turn
self.pub_wheels_done.publish(True)
#move forward
starting_time = rospy.Time.now()
while ((rospy.Time.now() - starting_time) <
rospy.Duration(forward_for_time_enter)):
wheels_cmd_msg = WheelsCmdStamped()
wheels_cmd_msg.header.stamp = rospy.Time.now()
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?.")
self.rate.sleep()
def rotate(self, velocity, time):
# print "Rotate"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = -velocity
self.wheelpub.publish(msg)
rospy.sleep(time)
message = "Rotate:done"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def forward(self, velocity, time):
# print "Forward"
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity
msg.vel_right = velocity
self.wheelpub.publish(msg)
rospy.sleep(time)
message = "distance: forward"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
self.estop = False
self.board_name = rospy.get_param("/board_name",
"smart_robot_omnibotV12")
self.port = "/dev/" + self.board_name
self.baud = 115200
# Setup publishers
self.driver = smart_robotV12(self.port, self.baud)
self.driver.connect()
# set system node ===============================
# vehicle (Bit0) : 0 -> omnibot ; 1 -> Mecanum ; 2 --> encoder , angle param and no imu(1C) ; 3 --> encoder , no angle parameter and no imu(1D) ( Ominibot V0.7 ) ;
# 4 --> no encoder , angle param , and no imu(1D) ; 5 --> no encoder , no angle parameter , and no imu(1D)
# imu (Bit3) : 0 -> not to do , 1 -> do it
# imu_axis (Bit4) : 0 -> not to do , 1 -> do it
# return_encoder(Bit6) : 0 -> not to do , 1 -> do it
# command (Bit7) : 0 -> control , 1 -> APP
# motor_direct (Bit8) : 0 -> normal , 1 -> reverse
# encoder_direct(Bit9) : 0 -> normal , 1 -> reverse
# turn_direct (Bit10) : 0 -> normal , 1 -> reverse
# imu_reverse (Bit11) : 0 -> normal , 1 -> reverse
#================================================
for i in range(2):
self.driver.set_system_mode(vehicle=3,
imu=0,
imu_axis=0,
return_encoder=0,
command=0,
motor_direct=0,
encoder_direct=0,
turn_direct=0,
imu_reverse=0) # use Ominibot V0.6
time.sleep(0.3)
#add publisher for wheels command wih execution time
self.msg_wheels_cmd = WheelsCmdStamped()
self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",
WheelsCmdStamped,
queue_size=1)
# Setup subscribers
self.control_constant = 1.0
self.sub_topic = rospy.Subscriber("~wheels_cmd",
WheelsCmdStamped,
self.cbWheelsCmd,
queue_size=1)
self.sub_e_stop = rospy.Subscriber("~emergency_stop",
BoolStamped,
self.cbEStop,
queue_size=1)
def __init__(self):
self.name = 'mdoap_controller_node'
rospy.loginfo('[%s] started', self.name)
self.sub_close = rospy.Subscriber("~too_close", BoolStamped, self.cbBool, queue_size=1)
self.sub_control = rospy.Subscriber("~lane_control", WheelsCmdStamped, self.cbLaneControl, queue_size=1)
self.sub_detections = rospy.Subscriber("~detection_list_proj", ObstacleProjectedDetectionList, self.cbDetections, queue_size=1)
self.pub_wheels_cmd = rospy.Publisher("~control",WheelsCmdStamped,queue_size=1)
self.too_close = False
self.lane_control = WheelsCmdStamped()
self.lane_control.vel_left = 0.0
self.lane_control.vel_right = 0.0
def test_v_dot_simple(self):
# publish wheel_cmd_executed
wheels_cmd = WheelsCmdStamped()
rate = rospy.Rate(10)
for i in range(10):
wheels_cmd.header.stamp = rospy.Time.now()
wheels_cmd.vel_left = 0.5
wheels_cmd.vel_right = 0.5
self.pub_wheels_cmd_executed.publish(wheels_cmd)
rate.sleep()
# publish LanePose
lane_pose = LanePose()
lane_pose.d = 0
lane_pose.sigma_d = 0
lane_pose.phi = 0
lane_pose.sigma_phi = 0
lane_pose.status = 0
lane_pose.in_lane = True
for i in [1, 2]:
lane_pose.header.stamp = rospy.Time.now()
self.pub_lane_pose.publish(lane_pose)
rate.sleep()
# publish StopLineReading
stop_line_reading = StopLineReading()
stop_line_reading.stop_line_detected = True
stop_line_reading.at_stop_line = False
stop_line_reading.stop_line_point = Point()
for x in [0.51, 0.5]:
stop_line_reading.header.stamp = rospy.Time.now()
stop_line_reading.stop_line_point.x = x
self.pub_stop_line_reading.publish(stop_line_reading)
rate.sleep()
# Wait for the odometry_training_pairs_node to finish starting up
timeout = time.time() + 2.0
while not self.sub_v_sample.get_num_connections() and \
not rospy.is_shutdown() and not time.time() > timeout:
rospy.sleep(0.1)
msg = self.v_received
# self.assertEqual(hasattr(msg,'d_L'),True)
self.assertEqual(msg.d_L, 0.5, 'd_L = %s' % msg.d_L)
self.assertEqual(msg.d_R, 0.5, 'd_R = %s' % msg.d_R)
self.assertAlmostEqual(msg.dt, 0.1, 2, 'dt = %s' % msg.dt)
self.assertEqual(
msg.theta_angle_pose_delta, 0,
'theta_angle_pose_delta = %s' % msg.theta_angle_pose_delta)
self.assertAlmostEqual(msg.x_axis_pose_delta, 0.01, 5,
'x = %s' % msg.x_axis_pose_delta)
self.assertEqual(msg.y_axis_pose_delta, 0,
'y = %s' % msg.y_axis_pose_delta)
def sub_dir_cb(self, msg):
drive_cmd = WheelsCmdStamped()
if msg.data == "left":
drive_cmd.vel_left = 0.0
drive_cmd.vel_right = 0.2
elif msg.data == "right":
drive_cmd.vel_left = 0.2
drive_cmd.vel_right = 0.0
else:
drive_cmd.vel_left = 0.35
drive_cmd.vel_right = 0.35
self.pub_drive_cmd_.publish(drive_cmd)
def continuous_publisher():
vel_pub = rospy.Publisher('/random/topic/', WheelsCmdStamped, queue_size=1)
rospy.init_node('continuous_test_publisher', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = WheelsCmdStamped()
msg.vel_left = np.random.random()
msg.vel_right = np.random.random()
vel_pub.publish(msg)
rate.sleep()
def threadSetSpeed(self):
#rospy.loginfo("[%s] Set speed " %(self.node_name))
left = int(-(self.v + self.omega) * self.kMaxPPMS)
right = int((self.v - self.omega) * self.kMaxPPMS)
self.ps.set_speed([left, right])
wheelcmd = WheelsCmdStamped()
wheelcmd.header.stamp = rospy.Time.now()
wheelcmd.vel_left = -left
wheelcmd.vel_right = right
self.pub_wheelcmd.publish(wheelcmd)
def right_turn(self, velocity, time):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = velocity + 0.3
msg.vel_right = velocity
self.wheelpub.publish(msg)
self.set_leds("right_turn")
rospy.sleep(time)
message = "right_turn"
rospy.loginfo("Publishing message: '%s'" % message)
self.chatterpub.publish(message)
def listener(msg):
global should_take_photo
vel_msg = WheelsCmdStamped()
if any(info.traffic_sign_type==5 for info in msg.infos):
os.system("~/killThing.sh")
time.sleep(3.63)
vel_msg.vel_right = 0.0
vel_msg.vel_left = 0.0
time.sleep(0.1)
vel.publish(vel_msg)
the_thing('JOYSTICK_CONTROL')
vel.publish(vel_msg)
should_take_photo=True
def __init__(self):
self.lane_pos_obj = 0.1
self.det_err_threshold = 0.2
self.det_err_slowdown = 3.0
self.corr_d = 3.0
self.corr_phi = 0.5
self.last_d = 0.0
self.last_phi = 0.0
self.vel_right = 0.0
self.vel_left = 0.0
self.vel_cmd = WheelsCmdStamped()
self.vel_pub = rospy.Publisher('/default/wheels_driver_node/wheels_cmd', WheelsCmdStamped, queue_size=1)
self.lane_reading = rospy.Subscriber("/default/lane_filter_node/lane_pose", LanePose, self.PoseHandling, queue_size=1)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(BraitenbergNode, self).__init__(node_name=node_name)
#self.veh_name = rospy.get_namespace().strip("/")
self.veh_name = str(os.environ['VEHICLE_NAME'])
self.msg_wheels_cmd = WheelsCmdStamped()
try:
self.mode = str(os.environ['mode'])
except:
self.log("No mode is specified. Mode is set to default (avoid).")
self.mode = 'avoid'
rospy.loginfo("Mode: {}".format(self.mode))
if self.mode not in ["avoid", "attract", "mixed"]:
rospy.loginfo("No correct mode is chosen. Mode is set to default (avoid).")
self.mode = 'avoid'
rospy.sleep(1)
self.rate = rospy.Rate(15)
# Use the kinematics calibration for the gain and trim
self.parameters['~gain'] = None
self.parameters['~trim'] = None
self.parameters['~baseline'] = None
self.parameters['~radius'] = None
self.parameters['~k'] = None
self.parameters['~limit'] = None
# Set parameters using a robot-specific yaml file if such exists
self.readParamFromFile()
self.updateParameters()
# Wait for the automatic gain control
# of the camera to settle, before we stop it
rospy.sleep(2)
rospy.set_param('/%s/camera_node/exposure_mode' %(self.veh_name) , 'off')
rospy.set_param('/%s/camera_node/res_w' %(self.veh_name), 320)
rospy.set_param('/%s/camera_node/res_h' %(self.veh_name), 240)
# Publisher for wheels command
self.pub_wheels = rospy.Publisher("/{}/wheels_driver_node/wheels_cmd".format(self.veh_name), WheelsCmdStamped, queue_size=1)
# Subscribers
self.sub_image = rospy.Subscriber("/{}/camera_node/image/compressed".format(self.veh_name), CompressedImage, self.callback, queue_size=1)
self.log("Initialized")
def test_april_tags_single_interval(self):
#Setup the publisher and subscribers
self.pub_april_tags = rospy.Publisher(
"visual_odometry_april_tags_node/april_tags",
AprilTags,
queue_size=1,
latch=True)
self.pub_wheels_cmd = rospy.Publisher(
"visual_odometry_april_tags_node/wheels_cmd",
WheelsCmdStamped,
queue_size=1,
latch=True)
# Wait for the forward_kinematics_node to finish starting up
timeout = time.time() + 5.0
while (self.pub_wheels_cmd.get_num_connections() < 1 or self.pub_april_tags.get_num_connections() < 1) and \
not rospy.is_shutdown() and not time.time()>timeout:
rospy.sleep(0.1)
# Publish a single wheels cmd, and two simple april tag messages
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.vel_left = 1
msg_wheels_cmd.vel_right = 1
self.pub_wheels_cmd.publish(msg_wheels_cmd)
rospy.sleep(0.2) #Wait so the tags come in the right order
T1 = Transform()
T2 = Transform()
T1.translation.y = 2
T2.translation.y = 2
T2.rotation.x, T2.rotation.y, T2.rotation.z, T2.rotation.w = tr.quaternion_about_axis(
-np.pi / 2, (0, 0, 1))
msg_tag1 = AprilTags()
tag = TagDetection()
tag.transform = T1
msg_tag1.detections.append(tag)
msg_tag1.header.stamp = rospy.Duration(0)
self.pub_april_tags.publish(msg_tag1)
rospy.sleep(0.2) #Wait so the tags come in the right order
msg_tag2 = AprilTags()
msg_tag1.header.stamp = rospy.Duration(1)
tag.transform = T2
msg_tag1.detections.append(tag)
self.pub_april_tags.publish(msg_tag1)
# Wait 1 second for the file to be output
rospy.sleep(3)
res = np.genfromtxt(
rospy.get_param("visual_odometry_april_tags_node/filename", ''))
assert_almost_equal(res, np.array([1, 1, 1, np.pi / 2, 2, 2]))
def continuous_publisher():
vehicle = os.getenv("VEHICLE_NAME")
topic = "/{}/wheels_driver_node/wheels_cmd".format(vehicle)
vel_pub = rospy.Publisher(topic, WheelsCmdStamped, queue_size=1)
rospy.init_node("random_action_node", anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = WheelsCmdStamped()
msg.vel_left = np.random.random()
msg.vel_right = np.random.random()
vel_pub.publish(msg)
rate.sleep()
def send_commands(self, wheel_cmds_dict):
vel_right_array = wheel_cmds_dict['right_wheel']
vel_left_array = wheel_cmds_dict['left_wheel']
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
for i in range(len(vel_right_array)):
msg_wheels_cmd.header.stamp = rospy.Time.now()
msg_wheels_cmd.vel_right = vel_right_array[i]
msg_wheels_cmd.vel_left = vel_left_array[i]
#rospy.loginfo("Left Wheel: {} \t Right Wheel: {}".format(vel_left_array[i], vel_right_array[i]))
self.pub_wheels_cmd.publish(msg_wheels_cmd)
rospy.sleep(1 / float(self.frequency))
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " %(self.node_name))
self.estop=False
# Setup publishers
self.driver = DaguWheelsDriver()
#add publisher for wheels command wih execution time
self.msg_wheels_cmd = WheelsCmdStamped()
self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed",WheelsCmdStamped, queue_size=1)
# Setup subscribers
self.control_constant = 1.0
self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.cbWheelsCmd, queue_size=1)
self.sub_e_stop = rospy.Subscriber("~emergency_stop", BoolStamped, self.cbEStop, queue_size=1)
def processBag(self):
count = 0
with rosbag.Bag(self.bagout, 'w') as outbag:
for topic, msg, t in rosbag.Bag(self.bagin).read_messages():
if rospy.is_shutdown():
break
if topic == self.topicin:
msgout = WheelsCmdStamped()
msgout.vel_left = msg.vel_left
msgout.vel_right = msg.vel_right
msgout.header.stamp = t
outbag.write(self.topicout, msgout, t)
count += 1
else:
outbag.write(topic, msg, t)
return count
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 = WheelsCmdStamped()
# Car Steering Mode
vel_left = (speed - steering)
vel_right = (speed + steering)
wheels_cmd_msg.header.stamp = self.joy.header.stamp
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):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % self.node_name)
self.estop = False
self.estop_stamp = rospy.get_rostime()
# Setup publishers
self.driver = DaguWheelsDriver()
# add publisher for wheels command wih execution time
self.msg_wheels_cmd = WheelsCmdStamped()
self.pub_wheels_cmd = rospy.Publisher("~wheels_cmd_executed", WheelsCmdStamped, queue_size=1)
# Setup subscribers
self.sub_topic = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.cbWheelsCmd, queue_size=1)
self.sub_e_stop = rospy.Subscriber("~emergency_stop", BoolStamped, self.cbEStop, queue_size=1)
self.sub_motor_rpm = rospy.Subscriber("~motor_rpm", Int16MultiArray, self.cbEncoderRPM, queue_size=1)
def send_commands(self, cmds):
"""
Publishes the wheel commands to ROS
"""
time = rospy.get_rostime()
cmd_msg = WheelsCmdStamped()
cmd_msg.header.stamp.secs = time.secs
cmd_msg.header.stamp.nsecs = time.nsecs
cmd_msg.vel_right = cmds[u'motor_right']
cmd_msg.vel_left = cmds[u'motor_left']
if self.nsent_commands == 0:
msg = 'ROSClient publishing first commands.'
logger.info(msg)
self.cmd_pub.publish(cmd_msg)
self.nsent_commands += 1
