def __init__(self):
self.speed = 0.0
self.turn = 0.0
self.lastUpdate = 0.0
rospy.init_node('robotControl', anonymous=True)
rospy.Subscribe("cmd_vel",Twist,self.cmd_vel_callback)
rospy.Subscribe("robotCommand",String,self.robCom_callback)
self.telem_pub = rospy.Publish("telemetry",String,queue_size=10)
self.robotCommand=rospy.Publish("robotCommand",String,queue_size=10)
def __init__(self):
self.pub = rospy.Publisher('analglyph', Image)
self.sub_depth = rospy.Subscribe('/kinect/depth/image_raw', Image, self.cb_depth)
self.sub_rgb = rospy.Subscribe('/kinect/rgb/image_raw', Image, self.cb_rgb)
self.bridge = CvBridge()
self.img_out = cv.CreateMat(480, 640, cv.CV_8UC3)
self.img_depth = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_r = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_g = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_b = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_rgb = cv.CreateMat(480, 640, cv.CV_8UC3)
def __init__(self):
self.depth_subscriber = rospy.Subscribe("camera/depth/image_rect_raw",
Image,
self.callback_depth_image)
self.bridge = CvBridge()
def __init__(self):
feedback_start = False
if rospy.has_param('start_tactile_feedback'):
feedback_start = rospy.get_param('start_tactile_feedback')
else:
print("Waiting for forward teleoperation finished")
if feedback_start:
self.ff_pub = rospy.Publisher(
"/sh_rh_ffj3_effort_controller/command",
Float64,
queue_size=10)
self.mf_pub = rospy.Publisher(
"/sh_rh_mfj3_effort_controller/command",
Float64,
queue_size=10)
self.rf_pub = rospy.Publisher(
"/sh_rh_rfj3_effort_controller/command",
Float64,
queue_size=10)
self.lf_pub = rospy.Publisher(
"/sh_rh_lfj3_effort_controller/command",
Float64,
queue_size=10)
self.lf_pub = rospy.Publisher(
"/sh_rh_lfj3_effort_controller/command",
Float64,
queue_size=10)
rospy.Subscribe("rh/tactile_filtered", BiotacAll,
self.tactile_callback)
rospy.Subscribe("rh/tactile_zero", Float64, self.zero_callback)
rospy.on_shutdown(self.release_tendon())
self.finger_name = ["ff", "mf", "rf", "lf", "th"]
self.pacs = {"ff": [], "mf": [], "rf": [], "lf": [], "th": []}
self.force_init = [0, 0, 0, 0, 0]
if rospy.has_param('contact_info'):
self.contact_info = rospy.get_param('contact_info')
else:
rospy.logerr("please set cotact info")
self.slipping = False
self.stable_grasp()
rospy.spin()
def main(mtx, dist):
cap = cv2.VideoCapture(0)
# ID 1 had changed for test purposes. TODO:Must fix before URC
aruco_dict = get_dictionary()
parameters = aruco.DetectorParameters_create()
parameters.markerBorderBits = 2
while not rospy.is_shutdown():
rospy.Subscribe('/stage_counter_topic', String, stage_callback)
print("L: " + str(leftId) + " R: " + print(rightId))
ret, frame = cap.read()
corners, ids, rejected_img_points = aruco.detectMarkers(gray, aruco_dict,
parameters=parameters,
cameraMatrix=mtx,
distCoeff=dist)
if np.all(ids is not None): # If there are markers found by detector
for i in range(0, len(ids)):
print(ids[i])
# Estimate pose of each marker and return the values rvec and tvec---different from camera coefficients
rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(corners[i], 0.02, mtx, dist)
(rvec - tvec).any() # get rid of that nasty numpy value array error
aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.01)
aruco.drawDetectedMarkers(frame, corners)
if len(ids) == 2:
w = frame.shape[1]
h = frame.shape[0]
x,y,r = find_center(corners[0])
coordinatePublisher.publish(str(x) +","+ str(y) + "," + str(w) + "," + str(h)+ "," + str(r))
x,y,r = find_center(corners[1])
coordinatePublisher1.publish(str(x) +","+ str(y) + "," + str(w) + "," + str(h) +"," + str(r))
elif len(ids) == 1:
w = frame.shape[1]
h = frame.shape[0]
x,y,r = find_center(corners[0])
coordinatePublisher.publish(str(x) +","+ str(y) + "," + str(w) + "," + str(h) + "," + str(r))
else:
coordinatePublisher.Publish('-')
coordinatePublisher1.Publish('-')
cv2.imshow('frame', frame)
dir_pub.Publish("1")
key = cv2.waitKey(3) & 0xFF
if key == ord('q'): # Quit
break
cap.release()
cv2.destroyAllWindows()
def __init__(self):
rospy.init_node('dbw_node')
self.dbw_enabled = False
self.linear_velocity = 0
self.angular_velocity = 0
self.current_velocity = 0
vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35)
fuel_capacity = rospy.get_param('~fuel_capacity', 13.5)
brake_deadband = rospy.get_param('~brake_deadband', .1)
decel_limit = rospy.get_param('~decel_limit', -5)
accel_limit = rospy.get_param('~accel_limit', 1.)
wheel_radius = rospy.get_param('~wheel_radius', 0.2413)
wheel_base = rospy.get_param('~wheel_base', 2.8498)
steer_ratio = rospy.get_param('~steer_ratio', 14.8)
max_lat_accel = rospy.get_param('~max_lat_accel', 3.)
max_steer_angle = rospy.get_param('~max_steer_angle', 8.)
self.steer_pub = rospy.Publisher('/vehicle/steering_cmd',
SteeringCmd,
queue_size=1)
self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd',
ThrottleCmd,
queue_size=1)
self.brake_pub = rospy.Publisher('/vehicle/brake_cmd',
BrakeCmd,
queue_size=1)
self.dbw_enable_sub = rospy.Subscribe('/vehicle/dbw_enabled', Boolean,
dbw_enabled_callback)
self.current_velocity_sub = rospy.Subscribe('/current_velocity',
TwistStamped,
current_velocity_callback)
self.twist_cmd_sub = rospy.Subscribe('/twist_cmd', TwistStamped,
twist_cmd_callback)
# TODO: Create `TwistController` object
self.controller = Controller(wheel_base, steer_ratio, 0, max_lat_accel,
max_steer_angle)
# TODO: Subscribe to all the topics you need to
self.loop()
def main():
print("Usage: demo.py [model path] [image topic] [output topic]")
model_path = sys.argv[1]
image_topic_name = sys.argv[2]
output_topic_name = sys.argv[3]
if (len(sys.argv) < 4):
print("arg error.")
return
pModel = load_model(model_path)
rospy.init("GAAS_ImageDetection_TEGU_demo" +
model_path) # which supports multi process.
pPublisher = rospy.Publisher(output_topic_name)
rospy.Subscribe(image_topic_name, 10, callback)
rospy.spin()
def __init__(self):
rospy.init_node('test_driver')
# max velocity supported by motor controller/platform
# velocity is in m/s
self.maxSupportedVelocity = rospy.get_param("~maxSupportedVelocity", 2.2352) # m/s
# max velocity the driver is permitted to drive
# velocity is in m/s
self.maxVelocity = rospy.get_param("~maxVelocity", 1)
# acceleration is in % from 0 to 100%
# for now, assume constant acceleration
self.accel = rospy.get_param("~accel", 50)
# min duration is the minimum duration to use
# in a motor command...this will allow for us to creep/crawl forward if needed
self.minDuration = rospy.get_param("~minDuration", 2)
self.timeToTurn = rospy.get_param("~timeToTurn", 2)
self.planSteps = []
self.mcPub = rospy.Publish("PhidgetMotor", MotorCommand)
self.laPub = rospy.Publish("PhidgetLinear", LinearCommand)
rospy.Subscribe('plan', Plan, handlePlan)
def listener():
rospy.init_node('listener')
rospy.Subscribe("depth_topic",depth,callback)
rospy.spin()
ros
def listener():
rospy.init_node('grabber', anonymous=True)
rospy.Subscribe("/cmd_vel", Twist, cmd_vel_callback)
rospy.Subscribe("/camera/color/raw_image", Image, image_callback)
import rospy
from std_msgs.msg import string
def callback(dat):
word = dat.data
rospy.loginfo(word)
if __name__ == '__main__':
rospy.init_node("simple_publisher_node")
try:
rospy.Subscribe('/hello', String, callback)
rospy.spin()
except rospy.ROSInterruptException:
pass
#!/usr/bin/env python
import agv5Int
import rospy
posSubs = rospy.Subscribe('/robotPos',)
if __name__ == '__main__':
def __init__(self, topic):
# 라이다 클래스 실행시 할거 쓰셈
rospy.Subscribe(topic, LaserScan, self.callback_lidar)
self.front = 0
forward_steer_topic = "FORWARD_STEER"
global forward_steer
global turn_offset
forward_steer = 0
turn_offset = 0
def update_turn_offset(offset):
global turn_offset
turn_offset = offset
rospy.loginfo("updated offset: %d", % (turn_offset));
def update_steer(steer):
global forward_steer
forward_steer = steer
rospy.loginfo("updated velocity: %d", % (forward_steer));
if __name__ == '__main__':
global forward_steer, turn_offset
rospy.init_node(NODE_NAME)
rospy.Subscribe(turn_offset_topic, Int32, update_turn_offset)
rospy.Subscribe(forward_steer_topic, Int32, update_steer)
i2c = smbus.SMBus(1)
DEVICE_ADDRESS = 0x0a
while True:
i2c.write_byte(DEVICE_ADDRESS, 99)
# vCom setting forward drive (0-255)
i2c.write_byte(DEVICE_ADDRESS, 200 + forward_steer)
# sCom setting steering (127 (standby), 126-0(max) left, 128-255(max) right)
i2c.write_byte(DEVICE_ADDRESS, 127 + turn_offset)
pass
left_side_step = Custom(json.setparse("21 Fst_L", offset=[darwin, hand]))
right_side_step = Custom(json.setparse("20 Fst_R", offset=[darwin, hand]))
l_step = Custom(json.setparse("24 F_E_L", offset=[darwin, hand]))
r_step = Custom(json.setparse("25 F_E_R", offset=[darwin, hand]))
kick = Custom(json.setparse("26 F_PShoot_R", offset=[darwin, hand]))
#-----------------------------------------------------------------------------------------------------------------------------------------------------
if __name__ == "__main__":
ddxl = Dynamixel(lock=20)
ddxl.angleWrite(19, 0)
ddxl.angleWrite(20, 50)
balance.run()
raw_input("Proceed?")
#rospy.init_node('Dash', anonymous=True)
pub = rospy.Subscriber('shift', Float64, move, queue_size=1)
pub_new = rospy.Subscribe('detect', String, det, queue_size=1)
while True:
if move > 0:
left_side_step.run()
if ret == "IR":
break
else:
right_side_step.run()
if ret == "IR":
break
rospy.spin()
def __init__(self):
# set about print
self.output_controller = False
self.output_constant_PID = True
# set constant value
self.buoyancy = -0.0
self.first_time = True
# set k pid for first time
self.Kp_velocity = [0.7, 0.7, 0.7, 0.15, 0.15, 0.1]
self.Ki_velocity = [0.05, 0.05, 0.0, 0.0, 0.0, 0.0]
self.Kd_velocity = [0.0, 0.0, 0.0, 0.1, 0.1, 0.1]
self.Kp_position = [0.7, 0.7, 3.0, 0.4, 0.3, 0.4]
self.Ki_position = [0.1, 0.1, 0.1, 0.1, 0.5, 0.01]
self.Kd_position = [0.2, 0.2, 0.2, 0.08, 0.2, 0.01]
self.eq = expansion_quaternion()
self.et = expansion_twist()
self.error_position = numpy.array([0, 0, 0, 0, 0,
0]) # x, y, z, roll, pitch, yaw
self.fix_state = [True, True, True, True, True,
True] # for fix x, y, z, roll, pitch, yaw
self.PID_position = []
self.PID_velocity = []
self.out_calculate = []
for i in range(0, 6):
self.PID_position.append(PID())
self.PID_velocity.append(PID())
self.out_calculate.append(0)
self.set_PID()
self.fix_orientation = Quaternion(0, 0, 0, 1)
rospy.init_node("controller")
rate = rospy.Rate(10) # 10 hz
#Listening rospy.Subscrive("topic name", variable type, function)
rospy.Subscribe("/auv/state", Odometry,
self.get_auv_state) #get current state
rospy.Subscribe("/zeabus/cmd_vel", Twist,
self.get_velocity) # get future state or goal
rospy.Subscribe("/cmd_fix_position", Point,
self.get_fix_position) # get x y z
rospy.Subscribe("/cmd_fix_orientation", Quaternion,
self.get_fix_orientation) # get
rospy.Subscribe("/zeabus_controller/mode", Int16, self.get_mode)
rospy.Subscribe("/fix/abs/depth", Float64, self.get_fix_depth)
rospy.Subscribe("/fix/rel/yaw", Float64, self.get_real_yaw)
rospy.Subscribe("/fix/abs/yaw", Float64, self.get_fix_yaw)
rospy.Subscribe("/fix/rel/x", Float64, self.get_real_x)
rospy.Subscribe("/switch/data", Switch, self.get_switch)
#Use service rospy.Service("service name", service name, function)
# self.result = rospy.Service("/fix_rel_x_srv", drive_x, self.request_xy) #return boolean get x y
#declare variable for publisher rospy.Publisher('topic name', variable type, size)
self.cmd_vel = rospy.Publisher("/cmd_vel", Twist,
queue_size=10) # send to trust mapper
self.fix_position_pubilsher = rospy.Publisher(
"/zeabus_controller/is_at_fix_position", Bool,
queue_size=10) # send for check True is success or fix position
self.fix_orientation_pubilsher = rospy.Publisher(
"/zeabus_controller/is_at_fix_orientation", Bool,
queue_size=10) # same above but change position to orientation
#declare about dynamic config and create
# self.server = dynamic_reconfigure_server(PID_dynamic, self.configurePID) # ("ConfigType", function to recieve value)
self.server = dynamic_reconfigure_server(
PIDConstantConfig,
self.configurePID) # ("ConfigType", function to recieve value)
def listener():
rospy.init_node("acceleration")
rospy.Subscribe("/mavros/local_position/velocity", TwistStamped,
velocity_update)
rospy.spin()
def _uavGetHeight(self):
rospy.Subscribe("/mavros/global_position/local",
PoseWithCovarianceStamped, self.callbackHeight)
def LidarCheck():
global Lcheck
rospy.Subscribe('/distance_sensor', Range, range_check)
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import turtlesim.msg
def pose_callback:
#do something -Broadcast transform
br = tf.TransformBroadcaster(msg, turtlename)
br.SendTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world")
if __name__ == "main":
rospy.init_node("tf_broadcast")
turtlename = rospy.get_param('~turtle')
rospy.Subscribe('/%s/pose' % turtlename, turtlesim.msg.pose, pose_callback, turtlename)
rospy.spin()
