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