def main():
try:
opts, args = getopt.getopt(sys.argv[1:], 'hc:',
['help', 'calibrate=',])
except getopt.GetoptError as err:
print str(err)
usage()
sys.exit(2)
arm = None
for o, a in opts:
if o in ('-h', '--help'):
usage()
sys.exit(0)
elif o in ('-c', '--calibrate'):
arm = a
if not arm:
usage()
rospy.logerr("No arm specified")
sys.exit(1)
rospy.init_node('calibrate_arm_sdk', anonymous=True)
rs = baxter_interface.RobotEnable()
rs.enable()
cat = CalibrateArm(arm)
rospy.loginfo("Running calibrate on %s arm" % (arm,))
error = None
try:
cat.run()
except Exception, e:
error = str(e)
def setHeadingServer():
#Initialise node
rospy.init_node('set_heading_service')
s = rospy.Service('set_heading_value', command_val, setHeadingValueHandler)
print "Ready to set throttle value"
rospy.spin()
def main():
rospy.init_node('state_machine')
sm = smach.StateMachine(outcomes=['END'])
with sm:
smach.StateMachine.add('LISTEN', Listen(), transitions={'Bell':'NAVIGATION'})
smach.StateMachine.add('NAVIGATION', Navigation(), transitions={'SPEECH':'SPEECH','RECOGNITION':'RECOGNITION','DELIMAN_IN_KITCHEN':'DELIMAN_IN_KITCHEN','DOCTOR_IN_BEDROOM':'DOCTOR_IN_BEDROOM','BYE':'BYE','Failed':'NAVIGATION','Unknown':'END'})
smach.StateMachine.add('RECOGNITION', Recognition(), transitions={'Doctor':'DOCTOR','Deliman':'DELIMAN','Postman':'POSTMAN','Unknown':'UNKNOWN_PERSON','Unrecognised':'SPEECH','No Face':'SPEECH','Unsure':'SPEECH'})
smach.StateMachine.add('CONFIRM_RECOGNITION', ConfirmRecognition(), transitions={'Doctor':'DOCTOR','Deliman':'DELIMAN','Postman':'POSTMAN','Unknown':'UNKNOWN_PERSON','Unrecognised':'RECOGNITION','Unsure':'CONFIRM_RECOGNITION'})
smach.StateMachine.add('SPEECH', SpeechState(), transitions={'NAVIGATION':'NAVIGATION','RECOGNITION':'RECOGNITION','LISTEN':'LISTEN','POSTMAN_ENTERED':'POSTMAN_ENTERED','POSTMAN_WAIT':'POSTMAN_WAIT','DELIMAN_ENTERED':'DELIMAN_ENTERED','DELIMAN_WAIT':'DELIMAN_WAIT','DOCTOR_ENTERED':'DOCTOR_ENTERED','DOCTOR_WAIT':'DOCTOR_WAIT','CONFIRM_RECOGNITION':'CONFIRM_RECOGNITION'})
smach.StateMachine.add('DOCTOR', Doctor(), transitions={'NAVIGATION':'NAVIGATION'})
smach.StateMachine.add('DELIMAN', Deliman(), transitions={'NAVIGATION':'NAVIGATION'})
smach.StateMachine.add('POSTMAN', Postman(), transitions={'NAVIGATION':'NAVIGATION'})
smach.StateMachine.add('UNKNOWN_PERSON', UnknownPerson(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('POSTMAN_ENTERED', PostmanEntered(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('POSTMAN_WAIT', PostmanWait(), transitions={'Leaving':'BYE'})
smach.StateMachine.add('DELIMAN_ENTERED', DelimanEntered(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DELIMAN_IN_KITCHEN', DelimanInKitchen(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DELIMAN_WAIT', DelimanWait(), transitions={'Leaving':'SPEECH'})
smach.StateMachine.add('DOCTOR_ENTERED', DoctorEntered(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DOCTOR_IN_BEDROOM', DoctorInBedroom(), transitions={'Speech':'SPEECH'})
smach.StateMachine.add('DOCTOR_WAIT', DoctorWait(), transitions={'Leaving':'SPEECH'})
smach.StateMachine.add('BYE', Bye(), transitions={'Speech':'SPEECH'})
rospy.sleep(2)
Recognition.unknown_count = 0
outcome = sm.execute()
def __init__(self, node_name):
rospy.init_node(node_name)
controller = Controller(rospy)
time.sleep(1)
print "Will now test moving forwards"
controller.move_forward(1)
time.sleep(1)
print "Will now test moving backwards"
controller.move_backwards(1)
time.sleep(1)
print "Will now test moving sideways"
controller.turn_left(1)
controller.move_forward(1)
time.sleep(2)
controller.turn_left(2)
controller.move_backwards(1)
time.sleep(1)
print "Will now play music"
controller.play_song()
print "See ya!"
def __init__(self):
rospy.init_node('actuators_handler')
rospy.loginfo(rospy.get_caller_id() + 'Initializing actuators_handler node')
self.timelast = 0
#Get all parameters from config (rosparam)
name = 'engine'
engine_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
engine_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 60))
engine_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))
name = 'steering'
steering_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
steering_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 62))
steering_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))
#Initialize PWM
self.dev1 = mraa.Pwm(engine_board_pin)
self.dev1.period_us(engine_period_us)
self.dev1.enable(True)
self.dev1.pulsewidth_us(1500)
self.dev2 = mraa.Pwm(steering_board_pin)
self.dev2.period_us(steering_period_us)
self.dev2.enable(True)
self.dev2.pulsewidth_us(1500)
def __init__(self, node_name_override = 'imu_node'):
rospy.init_node(node_name_override)
self.nodename = rospy.get_name()
rospy.loginfo("imu_node starting with name %s", self.nodename)
self.rate = rospy.get_param("param_global_rate", 10)
self.init_time = rospy.get_time()
self.roll = 0
self.pitch = 0
self.yaw = 0
self.grad2rad = 3.141592/180.0
self.imuPub = rospy.Publisher('imu_topic', Imu)
self.imuMsg = Imu()
self.imuMsg.orientation_covariance = [999999 , 0 , 0, 0, 9999999, 0, 0, 0, 999999]
self.imuMsg.angular_velocity_covariance = [9999, 0 , 0, 0 , 99999, 0, 0 , 0 , 0.02]
self.imuMsg.linear_acceleration_covariance = [0.2 , 0 , 0, 0 , 0.2, 0, 0 , 0 , 0.2]
self.imuSub = rospy.Subscriber("imu_data", float32_12, self.imuDataCb)
self.yaw = 0.001
self.pitch = 0.001
self.roll = 0.001
self.q = tf.transformations.quaternion_from_euler(self.roll,self.pitch,self.yaw)
self.imuMsg.orientation.x = self.q[0]
self.imuMsg.orientation.y = self.q[1]
self.imuMsg.orientation.z = self.q[2]
self.imuMsg.orientation.w = self.q[3]
self.imuMsg.header.stamp= rospy.Time.now()
self.imuMsg.header.frame_id = 'base_link'
self.imuPub.publish(self.imuMsg)
def main():
rospy.init_node(NODE_NAME)
janus_host = required_param('/lg_mirror/janus_stream_host', str)
janus_port = required_param('~janus_port', int)
device = rospy.get_param('~device', '/dev/capture_cam')
width = int(rospy.get_param('~width'))
height = int(rospy.get_param('~height'))
framerate = int(rospy.get_param('~framerate'))
max_quantizer = int(rospy.get_param('~max_quantizer', 60))
target_bitrate = int(rospy.get_param('~target_bitrate', 768000))
flip = rospy.get_param('~flip', False) in ['true', 'True', 't', 'yes', 'flipped', True]
capture = CaptureWebcam(
janus_host,
janus_port,
device,
width,
height,
framerate,
max_quantizer,
target_bitrate,
flip,
)
capture.start_capture()
rospy.spin()
def __init__(self):
rospy.init_node('Gocrazy', anonymous=False)
self.distance = 0
self.angle = 0
rospy.loginfo("To stop TurtleBot CTRL + C")
# What function to call when you ctrl + c
rospy.on_shutdown(self.shutdown)
# Create a publisher which can "talk" to TurtleBot and tell it to move
# Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if
# you're not using TurtleBot2
self.cmd_vel = rospy.Publisher(
'cmd_vel_mux/input/navi', Twist, queue_size=10)
# TurtleBot will stop if we don't keep telling it to move. How often
# should we tell it to move? 10 HZ
r = rospy.Rate(10)
move_cmd = Twist()
move_cmd.linear.x = self.distance
move_cmd.angular.y = self.angle
while not rospy.is_shutdown():
# publish the velocity
self.cmd_vel.publish(move_cmd)
# wait for 0.1 seconds (10 HZ) and publish again
r.sleep()
def __init__(self):
"JoyNode constructor"
# estop controls
self.run = 3 # start autonomous run
self.suspend = 12 # suspend autonomous running
self.estop = 13 # emergency stop
# tele-operation controls
self.steer = 0 # steering axis (wheel)
self.drive = 4 # shift to Drive (^)
self.reverse = 6 # shift to Reverse (v)
self.park = 7 # shift to Park
self.throttle = 18 # throttle axis (X)
self.throttle_start = True
self.brake = 19 # brake axis (square)
self.brake_start = True
self.lowincrease_max = 11 # Increase max .5 (R1)
self.highincrease_max = 9 # Increase max 2 (R2)
self.lowdecrease_max = 10 # Decrease max .5 (L1)
self.highdecrease_max = 8 # Decrease max 2 (L2)
# initialize ROS topics
rospy.init_node('josh_teleop')
self.pilot = pilot_cmd.PilotCommand()
self.reconf_server = ReconfigureServer(JoyConfig, self.reconfigure)
self.joy = rospy.Subscriber('joy', Joy, self.joyCallback)
def main():
rospy.init_node('pose2odom')
# global cov_thresh
# cov_thresh = rospy.get_param("~cov_thresh", 10**2)
rospy.Subscriber("ndt_pose", PoseStamped, callback)
rospy.spin()
def __init__(self, robot_name, ros_namespace = '/dvrk/'):
"""Constructor. This initializes a few data members.It
requires a robot name, this will be used to find the ROS
topics for the robot being controlled. For example if the
user wants `PSM1`, the ROS topics will be from the namespace
`/dvrk/PSM1`"""
# data members, event based
self.__robot_name = robot_name
self.__ros_namespace = ros_namespace
self.__robot_state = 'uninitialized'
self.__robot_state_event = threading.Event()
self.__goal_reached = False
self.__goal_reached_event = threading.Event()
# continuous publish from dvrk_bridge
self.__position_joint_desired = []
self.__effort_joint_desired = []
self.__position_cartesian_desired = Frame()
self.__position_joint_current = []
self.__velocity_joint_current = []
self.__effort_joint_current = []
self.__position_cartesian_current = Frame()
# publishers
frame = Frame()
full_ros_namespace = self.__ros_namespace + self.__robot_name
self.set_robot_state_publisher = rospy.Publisher(full_ros_namespace + '/set_robot_state',
String, latch=True, queue_size = 1)
self.set_position_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_joint',
JointState, latch=True, queue_size = 1)
self.set_position_goal_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_joint',
JointState, latch=True, queue_size = 1)
self.set_position_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_cartesian',
Pose, latch=True, queue_size = 1)
self.set_position_goal_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_cartesian',
Pose, latch=True, queue_size = 1)
self.set_jaw_position_publisher = rospy.Publisher(full_ros_namespace + '/set_jaw_position',
Float32, latch=True, queue_size = 1)
self.set_wrench_body_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_body',
Wrench, latch=True, queue_size = 1)
self.set_wrench_spatial_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_spatial',
Wrench, latch=True, queue_size = 1)
# subscribers
rospy.Subscriber(full_ros_namespace + '/robot_state',
String, self.__robot_state_callback)
rospy.Subscriber(full_ros_namespace + '/goal_reached',
Bool, self.__goal_reached_callback)
rospy.Subscriber(full_ros_namespace + '/state_joint_desired',
JointState, self.__state_joint_desired_callback)
rospy.Subscriber(full_ros_namespace + '/position_cartesian_desired',
Pose, self.__position_cartesian_desired_callback)
rospy.Subscriber(full_ros_namespace + '/state_joint_current',
JointState, self.__state_joint_current_callback)
rospy.Subscriber(full_ros_namespace + '/position_cartesian_current',
Pose, self.__position_cartesian_current_callback)
# create node
# rospy.init_node('robot_api', anonymous = True)
rospy.init_node('robot_api',anonymous = True, log_level = rospy.WARN)
rospy.loginfo(rospy.get_caller_id() + ' -> started robot: ' + self.__robot_name)
def main():
global client
try:
rospy.init_node("test_move", anonymous=True, disable_signals=True)
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
print "Waiting for server..."
client.wait_for_server()
print "Connected to server"
while 1:
#move1()
#move2()
#print 'Enter Tra Array:'
#s = raw_input()
#split1 = s.split(" ")
#for i in split1:
# Q.append(float(i))
#move1()
#del Q[0:len(Q)]
#move1()
#move_repeated()
#move_disordered()
#move_interrupt()
#move1()
#move_repeated()
#move_disordered()
#move_interrupt()
except KeyboardInterrupt:
rospy.signal_shutdown("KeyboardInterrupt")
raise
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = rospy.Publisher('image_raw', Image)
self.pub_info_ = rospy.Publisher('camera_info', CameraInfo)
self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
# Messages
self.info_ = CameraInfo()
self.set_default_params_qvga(self.info_) #default params should be overwritten by service call
# parameters
self.camera_switch = rospy.get_param('~camera_switch', 0)
if self.camera_switch == 0:
self.frame_id = "/CameraTop_frame"
elif self.camera_switch == 1:
self.frame_id = "/CameraBottom_frame"
else:
rospy.logerr('Invalid camera_switch. Must be 0 or 1')
exit(1)
print "Using namespace ", rospy.get_namespace()
print "using camera: ", self.camera_switch
#TODO: parameters
self.resolution = kQVGA
self.colorSpace = kBGRColorSpace
self.fps = 30
# init
self.nameId = ''
self.subscribe()
def talker(data):
pub = rospy.Publisher('chatter', Int64, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
rospy.loginfo(data)
pub.publish(data)
rate.sleep()
def __init__(self, image_topic):
""" Initialize """
rospy.init_node("digit_reader")
# Load machine learning model
path = rospkg.RosPack().get_path("text_recognition")
self.model = joblib.load("{}/model/model.pkl".format(path))
# Bridge to convert ROS messages to OpenCV
self.bridge = CvBridge()
# To filter out noise when there is not a digit in the frame,
# we only accept predictions when the confidence is high enough.
self.min_digit_confidence = 0.5
# We also use a sliding window and only accept the prediction when there is agreement
self.max_len = 5
self.last_digits = deque([-1], maxlen=self.max_len)
# Target angle, error from current angle, allowed error, and list of set points
self.target_angle = 0
self.angle_error = 0
self.max_error = 0.075
self.angles = [angle_normalize(-i * np.pi / 5) for i in range(10)]
# Proportional control constant
self.k_p = 0.5
# Flag so we can toggle between reading and turning
self.is_turning = False
# Subscribe to the image topic and create a cmd_vel publisher
rospy.Subscriber(image_topic, Image, self.process_image)
rospy.Subscriber("/odom", Odometry, self.process_odom)
self.pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
def main():
rospy.init_node('assignment10_trajectory')
path_reader = PathReader('sample_map_origin_map.txt') # constructor creates publishers / subscribers
rate = rospy.Rate(1)
while not rospy.is_shutdown():
path_reader.publish()
rate.sleep()
def __init__(self):
rospy.init_node('turtlebot_teleop')
self.l_scale = rospy.get_param('~drive_scale',0.6)
self.a_scale = rospy.get_param('~turn_scale',0.9)
self.deadman_button = rospy.get_param('~deadman_button', 0)
self.cmd = None
#publish cmd in Twist type to topic /cmd_vel
cmd_pub = rospy.Publisher('~cmd_vel', Twist)
# announce_pub = rospy.Publisher('/turtlebot_teleop/anounce/teleops',String, latch=True)
# anounce_pub.publisher(rospy.get_namespace());
rospy.Subscriber("joy", Joy, self.callback)
# rate = rospy.Rate(rospy.get_param('~hz', 20))
rate = rospy.Rate(10) #10hz
#keep sending cmd value while not shutdown
while not rospy.is_shutdown():
if self.cmd:
cmd_pub.publish(self.cmd)
rate.sleep()
def __init__(self):
rospy.init_node('phase_shift_service')
#self.start = time.clock()
#self.end = time.clock()
#self.timestamps = []
#self.actual_stamps = []
#self.actual_position = [0,0,0]
#self.hydro0 = [0, 0, 0]
#self.hydro1 = [-25.4, 0, 0]
#self.hydro2 = [25.4, 0, 0]
#self.hydro3 = [0, -25.4, 0]
#rospy.Subscriber('hydrophones/hydrophone_locations', Hydrophone_locations, self.hydrophone_locations)
#rospy.Subscriber('/hydrophones/actual_time_stamps', Actual_time_stamps, self.actual)
#rospy.Subscriber('/hydrophones/ping', Ping, self.parse_ping)
#self.calc_stamps_pub = rospy.Publisher('/hydrophones/calculated_time_stamps', Calculated_time_stamps, queue_size = 1)
rospy.Service('/hydrophones/calculated_time_stamps', Calculated_time_stamps_service, self.calculate_time_stamps_phase)
self.W = '\033[0m' # white (normal)
self.R = '\033[31m' # red
self.G = '\033[32m' # green
self.O = '\033[43m' # orange
self.B = '\033[34m' # blue
self.P = '\033[35m' # purple
rate = rospy.Rate(1) #rate of signals, 5 Hz for Anglerfish
while not rospy.is_shutdown():
rate.sleep()
def set_anchor_configuration():
tag_ids = [None]
rospy.init_node('uwb_configurator')
rospy.loginfo("Configuring device list.")
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
pozyx.doDiscovery(discovery_type=pypozyx.POZYX_DISCOVERY_TAGS_ONLY)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
tag_ids += device_list.data
for tag in tag_ids:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO,
len(anchors), remote_id=tag)
pozyx.saveRegisters(settings_registers, remote_id=tag)
pozyx.saveNetwork(remote_id=tag)
if tag is None:
rospy.loginfo("Local device configured")
else:
rosply.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
def __init__(self):
rospy.init_node("unit_tester")
# Misc Objects
self.point_cloud_generator = temp_GenerateBlockPoints(16)
print "> Initialization Complete."
def __init__(self):
# ROS initialization:
rospy.init_node('pose_manager')
self.poseLibrary = dict()
self.readInPoses()
self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
execute_cb=self.executeBodyPose,
auto_start=False)
self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
try:
rospy.wait_for_service("stop_walk_srv", timeout=2.0)
self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
except:
rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
+"This is normal if there is no nao_walker running.")
self.stopWalkSrv = None
self.poseServer.start()
rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());
else:
rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
rospy.signal_shutdown("Required component missing");
def __init__(self):
self.twistMessage = Twist()
self.pointMessage = PointStamped()
self.location_publisher = rospy.Publisher("location", Twist, queue_size = 10)
self.point_pub = rospy.Publisher("rviz_point", PointStamped, queue_size= 10)
rospy.init_node("location_node")
self.location_service = rospy.Service('locationService', location_srv, self.handle_location)
def __init__(self):
rospy.init_node('vel_def')
self.scale_rotRob = rospy.get_param("~scaleRotRob",0.5)
self.sigmaVel = rospy.get_param("~sigmaVel",0.5)
self.minVel = rospy.get_param("~minVel",0.025)
self.maxVel = rospy.get_param("~maxVel",0.3)
self.minVelForRot = rospy.get_param("~minVelForRot",0.08)
self.maxRotRob = rospy.get_param("~maxRotRob",0.5)
self.maxRotCam = rospy.get_param("~maxRotCam",0.5)
self.scale_lin = rospy.get_param("~scale_lin",0.5)
self.sigmaRot = rospy.get_param("~sigmaRot",0.08)
self.scale_rotCam = rospy.get_param("~scaleRotCam",0.5)
self.scale_rotInit = rospy.get_param("~scaleRotInit",1)
rospy.Subscriber("~servo", Twist, self.convert_vel)
sub = rospy.Subscriber('~joy', Joy, self.joy_cb)
sub = rospy.Subscriber('/imu/data', Imu, self.imu_cb)
self.rob_twist_pub = rospy.Publisher("~rob_twist_pub", Twist)
self.pan_pub_fl = rospy.Publisher("~pan_pub_float",Float64)
self.pan_pub_tw = rospy.Publisher("~pan_pub_twist",Twist)
self.ang_rob = 0
self.omega_z = 0
self.reached_goal = 0
self.reconfig_srv = Server(VelConvertConfig, self.reconfig_cb)
rospy.sleep(1.0)
def listener_node():
rospy.init_node('listener_node', anonymous=True)
rospy.Subscriber("move_base/status", GoalStatusArray, move_base_callback)
rospy.Subscriber("controller_cmd_vel", Twist, cmd_vel_callback)
rospy.spin()
def complete_timing_trials():
rospy.init_node('time_trials')
times = dict(zip(['pickup', 'place'], [[], []]))
awesome = pick_and_place()
for i in range(5):
#raw_input("pick up object?")
start = rospy.get_rostime().to_sec()
result = awesome.pick_up()
while not result:
result = awesome.pick_up()
continue
end = rospy.get_rostime().to_sec()
times['pickup'].append(int(end - start)+1) # ceiling round off
#raw_input("place object?")
rospy.sleep(1) # wait for robot to see AR tag
start = rospy.get_rostime().to_sec()
result = awesome.place(use_offset=False)
while not result:
result = awesome.place()
end = rospy.get_rostime().to_sec()
times['place'].append(int(end - start)+1) # ceiling round off
with open("time_results.csv", "wb") as f:
for key in times:
f.write( "%s, %s\n" % (key, ", ".join(repr(e) for e in times[key])))
def main():
# Initialize the ANNs
agent = DDPG()
rospy.init_node("neuro_deep_planner", anonymous=False)
ros_handler = ROSHandler()
ros_handler.on_policy = False
while not rospy.is_shutdown():
# If we have a new msg we might have to execute an action and need to put the new experience in the buffer
if ros_handler.new_msg():
if not ros_handler.is_episode_finished:
# Send back the action to execute
ros_handler.publish_action(agent.get_action(ros_handler.state))
# Safe the past state and action + the reward and new state into the replay buffer
agent.set_experience(ros_handler.state, ros_handler.reward, ros_handler.is_episode_finished)
elif ros_handler.new_setting():
agent.noise_flag = ros_handler.noise_flag
else:
# Train the network!
agent.train()
def __init__(self):
global publisher
rospy.init_node('pan_tilt_node')
config = PanTiltConfig()
# I2C Mode: 0 - RPI GPI, 1 - Adafruit 16-channel I2C controller
config.i2c_mode = int(self.get_param("i2c_mode", "1"))
config.pan_pin = int(self.get_param("pan_pin", "0"))
config.tilt_pin = int(self.get_param("tilt_pin", "1"))
config.pan_left_limit = int(self.get_param("pan_left_limit", "90"))
config.pan_right_limit = int(self.get_param("pan_right_limit", "210"))
config.tilt_down_limit = int(self.get_param("tilt_down_limit", "125"))
config.tilt_up_limit = int(self.get_param("tilt_up_limit", "185"))
config.pan_center = int(self.get_param("pan_center", "150"))
config.tilt_center = int(self.get_param("tilt_center", "155"))
# publish joint states to sync with rviz virtual model
# the topic to publish to is defined in the source_list parameter
# as: rosparam set source_list "['joints']"
publisher = rospy.Publisher("/joints", JointState)
i2cMessage.i2cthread = self
# subscribed to joystick inputs on topic "joy"
self.pub = rospy.Publisher("/i2cmove", vision.msg.I2CMove)
rospy.Subscriber("/joy", Joy, callback)
self.thread = BlinkThread()
self.thread.setDaemon(True)
self.thread.start()
def main():
#initiate ros, robot, assign variables...
rospy.init_node("rsdk_set_position")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
rs.enable()
r_axis=[1,0,0]
r_angle=pi/2
[x,y,z,w]=get_quaternion(r_axis, r_angle)
[px,py]=find_centre(get_object_position("Coaster"))
print "object location: x: ",px," y: ",py
#~ move_arm('left',ik_test("left",0.4605,0.1805,Z_START-0.3,x,y,z,w),4000)
move_arm('left',ik_test("left",px,py,Z_START-0.3,x,y,z,w),800)
move_arm('left',ik_test("left",px,py,Z_START-0.3,x,y,z,w),400)
r_axis=[1,0,0]
r_angle=pi
[x,y,z,w]=get_quaternion(r_axis, r_angle)
move_arm('left',ik_test("left",px,py,Z_START,x,y,z,w),800)
move_arm('left',ik_test("left",px,py,Z_START-0.2,x,y,z,w),800)
return 0
def __init__(self):
rospy.Subscriber("/speech_text", String, self.callback)
rospy.init_node('simple_chart_parser_node', anonymous=True)
self.path = os.path.join(rospy.get_param("/simple_chart_parser/grammar_path"), "grammars", rospy.get_param("/simple_chart_parser/grammar_name"))
self.grammar = nltk.data.load('file:%s'%self.path)
self.parser = nltk.ChartParser(self.grammar)
rospy.spin()
def main():
rospy.init_node("find_fiducial_pose_test")
# Construct action ac
rospy.loginfo("Starting action client...")
action_client = actionlib.SimpleActionClient("find_fiducial_pose", FindFiducialAction)
action_client.wait_for_server()
rospy.loginfo("Action client connected to action server.")
# Call the action
rospy.loginfo("Calling the action server...")
action_goal = FindFiducialGoal()
action_goal.camera_name = "/camera/rgb"
action_goal.pattern_width = 7
action_goal.pattern_height = 6
action_goal.pattern_size = 0.027
action_goal.pattern_type = 1 # CHESSBOARD
if (
action_client.send_goal_and_wait(action_goal, rospy.Duration(50.0), rospy.Duration(50.0))
== GoalStatus.SUCCEEDED
):
rospy.loginfo("Call to action server succeeded")
else:
rospy.logerr("Call to action server failed")
if self._dist_to_goal[max(self._dist_to_goal)] < 0.2:
self.flag['preland'] = 1
def iteration(self, event):
# publish goal messages for each uav
self.publish_msg()
# publish rotor velocities for each uav
for id in enumerate(self.cf_ids):
self.update_rotor_vels(id)
if __name__ == '__main__':
# write code to create PositionControllerNode_ChihChun
rospy.init_node('position_controller_node_ChihChun_flocking',
disable_signals=True)
ids = [1, 2]
initials = np.array([[0, 0, 0], [1, 1, 0]])
finals = np.array([[1, 1, 1], [0, 0, 1]])
vx_ds = [0.0, 0.0] # desired vx
vy_ds = [0.0, 0.0] # desired vy
vz_ds = [0.0, 0.0] # desired vz
yaw_ds = [0.0, 0.0] # desired yaw
dt = 1.0 / 15
cf_flocking = position_controller_flock_node(ids, initials, finals, vx_ds,
vy_ds, vz_ds, yaw_ds)
rospy.Timer(rospy.Duration(dt), cf_flocking.iteration)
rospy.spin()
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Point
from sensor_msgs.msg import PointCloud
import std_msgs.msg
if __name__ == '__main__':
rospy.init_node('pixel_to_coordinate_calculator')
cloud_pub = rospy.Publisher(
'pointcloud_debug',
PointCloud,
queue_size=10
)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
debug_pointcloud = PointCloud()
debug_pointcloud.header = std_msgs.msg.Header()
debug_pointcloud.header.stamp = rospy.Time.now()
debug_pointcloud.header.frame_id = "laser"
number_of_pixels = 10
# create an empty list of correct size
debug_pointcloud.points = [None] * number_of_pixels
# fill up pointcloud with points where x value changes but y and z are all 0
for p in xrange(0, number_of_pixels):
debug_pointcloud.points[p] = Point(p, 0, 0)
def closeEvent(self, event):
self.save_ui_info()
def load_ui_info(self):
settings = SettingsManager(CONFIG_FILE)
settings.load(self.ui.load_path_edit)
settings.load(self.ui.override_velocity_edit)
def save_ui_info(self):
settings = SettingsManager(CONFIG_FILE)
settings.save(self.ui.load_path_edit)
settings.save(self.ui.override_velocity_edit)
if __name__ == '__main__':
rospy.init_node('waypoint_player')
app = QApplication(sys.argv)
lock_manager = LockManager(LOCK_PATH)
if not lock_manager.get_lock():
QMessageBox.warning(
None, 'Warning',
'Another same process is running. If not, please delete lock file [ %s ].'
% lock_manager.get_lock_path(), QMessageBox.Ok)
sys.exit(1)
win = WaypointPlayerDialog()
win.show()
ret = app.exec_()
lock_manager.release_lock()
#!/usr/bin/env python
import rospy
import cv2
import numpy as np
import os, rospkg
import json
from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridgeError
from utils import BEVTransform, CURVEFit, draw_lane_img
from image_parser import IMGParser
if __name__ == '__main__':
rp = rospkg.RosPack()
currentPath = rp.get_path("lane_detection_example")
with open(os.path.join(currentPath, 'sensor/sensor_params.json'), 'r') as fp:
sensor_params = json.load(fp)
params_cam = sensor_params["params_cam"]
rospy.init_node('image_parser', anonymous=True)
image_parser = IMGParser()
bev_op = BEVTransform(params_cam=params_cam)
curve_learner = CURVEFit(order=3)
rate = rospy.Rate(20)
var = msg.data
if var <= 2500:
#send message to turn OFF the LED
varP=0
rospy.loginfo("The output is OFF and the var is: %s", var)
else:
#send message to turn ON the LED
varP=1
rospy.loginfo("The output is ON and the var is: %s", var)
pub.publish(varP)
if __name__=='__main__':
rospy.init_node('random_LED')
rospy.Subscriber('random_number',Int32, callback)
pub=rospy.Publisher('LED', Int32, queue_size=1)
# rate=rospy.Rate(10)
rospy.spin()
#while not rospy.is_shutdown():
# if var <= 2500:
# #send message to turn OFF the LED
# varP="OFF"
# rospy.loginfo("The output is OFF and the var is: %s", var)
# else:
# #send message to turn ON the LED
# varP="ON"
# rospy.loginfo("The output is ON and the var is: %s", var)
def __remove_free_space(name):
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
co.type.db = json.dumps({
'table': 'NONE',
'type': 'free_space',
'name': co.id
})
psw = PlanningSceneWorld()
psw.collision_objects.append(co)
obstacle_pub.publish(psw)
rospy.loginfo("Removed a fake obstacle named '%s'", name)
rospy.init_node('fake_obstacle')
# Publish fake collision objects to the planning scene world; add and remove
obstacle_pub = rospy.Publisher(
'move_base_flex/local_costmap/semantic_layer/add_objects',
PlanningSceneWorld,
queue_size=1)
rospy.sleep(2)
__add_free_space('KK1', 2, 1.0)
rospy.sleep(5)
__add_free_space('KK2', 2.8, 2.0)
rospy.sleep(5)
__add_free_space('KK3', 0.6, 0.2)
rospy.sleep(5)
__add_free_space('KK4', 2.6, 0.8)
import rospy
from clever import srv
LED_COUNT = 6 # Number of LED pixels.
LED_PIN = 18 # GPIO pin connected to the pixels (18 uses PWM!).
#LED_PIN = 10 # GPIO pin connected to the pixels (10 uses SPI /dev/spidev0.0).
LED_FREQ_HZ = 800000 # LED signal frequency in hertz (usually 800khz)
LED_DMA = 10 # DMA channel to use for generating signal (try 10)
LED_BRIGHTNESS = 255 # Set to 0 for darkest and 255 for brightest
LED_INVERT = False # True to invert the signal (when using NPN transistor level shift)
LED_CHANNEL = 0
import neopixel
rospy.init_node('Led')
neopixel.strip = neopixel.Adafruit_NeoPixel(LED_COUNT, LED_PIN, LED_FREQ_HZ, LED_DMA, LED_INVERT, LED_BRIGHTNESS, LED_CHANNEL)
neopixel.strip.begin()
def fun_led(req):
if req.fun == "green":
color = neopixel.Color(255,0,0)
for i in range(0,LED_COUNT):
neopixel.strip.setPixelColor(i,color)
neopixel.strip.show()
elif req.fun == "red":
color = neopixel.Color(0,255,0)
for i in range(0,LED_COUNT):
neopixel.strip.setPixelColor(i,color)
neopixel.strip.show()
self.move.target_pose.header.frame_id = 'odom_combined'
self.move.target_pose.pose.position.x = 10
self.move.target_pose.pose.position.y = 10
self.move.target_pose.pose.orientation.w = 1
self.ac_door = SimpleActionClient('move_through_door', DoorAction)
print 'Waiting for open door action server'
self.ac_door.wait_for_server()
self.ac_move = SimpleActionClient('move_base_local', MoveBaseAction)
print 'Waiting for move base action server'
self.ac_move.wait_for_server()
rospy.Subscriber("/test_output", String, self.stringOutput)
def stringOutput(self, str):
print str.data
def test_door_no_executive(self):
self.assert_(self.ac_door.send_goal_and_wait(self.door, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
self.assert_(self.ac_move.send_goal_and_wait(self.move, rospy.Duration(TEST_DURATION), rospy.Duration(5.0)))
if __name__ == '__main__':
rospy.init_node('open_door_test', anonymous=True)
rostest.run(PKG, sys.argv[0], TestDoorNoExecutive, sys.argv) #, text_mode=True)
def __init__(self):
rospy.init_node('thrusterNode', anonymous=False)
markerPub = rospy.Publisher('thruster_marker_topic', Marker, queue_size=10)
self.accelPub = rospy.Publisher('accel_topic', Accel, queue_size=10)
rospy.Subscriber("cmd_vel", Twist, self.move_callback)
tf_broadcaster = tf.TransformBroadcaster()
thruster_frame_id = rospy.get_param('~thruster_frame_id', 'thruster')
thruster_parent_frame_id = rospy.get_param('~thruster_parent_frame_id', 'rocket')
refresh_rate = 25.0
rate = rospy.Rate(refresh_rate)
dT = 1.0/refresh_rate
self.robotFireMarker = Marker()
self.robotFireMarker.header.frame_id = thruster_frame_id
self.robotFireMarker.header.stamp = rospy.get_rostime()
self.robotFireMarker.ns = "game_markers"
self.robotFireMarker.id = 6
self.robotFireMarker.type = 3 # cylinder
self.robotFireMarker.action = 0
self.robotFireMarker.pose.position.x = 0.0
self.robotFireMarker.pose.position.y = 0.0
self.robotFireMarker.pose.position.z = 1
self.robotFireMarker.pose.orientation.x = 0
self.robotFireMarker.pose.orientation.y = 0.7071068
self.robotFireMarker.pose.orientation.z = 0
self.robotFireMarker.pose.orientation.w = 0.7071068
self.robotFireMarker.scale.x = 1.0
self.robotFireMarker.scale.y = 1.0
self.robotFireMarker.scale.z = 3.0
self.robotFireMarker.color.r = 235.0/255.0
self.robotFireMarker.color.g = 73.0/255.0
self.robotFireMarker.color.b = 52.0/255.0
self.robotFireMarker.color.a = 1.0
self.robotFireMarker.lifetime = rospy.Duration(0.0)
self.robotFireMarker.frame_locked = True
self.thrust = 0.0
self.thrust_zeroing_counter = 0
while not rospy.is_shutdown():
self.thrust_zeroing_counter += 1
if self.thrust_zeroing_counter > refresh_rate/2:
self.thrust_zeroing_counter = refresh_rate/2
self.thrust = 0.0
self.robotFireMarker.header.stamp = rospy.get_rostime()
markerPub.publish(self.robotFireMarker)
tf_broadcaster.sendTransform((-self.thrust*2, 0.0, 1.0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
thruster_frame_id,
thruster_parent_frame_id)
rate.sleep()
from rosbridge_server import RosbridgeWebSocket
from rosbridge_library.capabilities.advertise import Advertise
from rosbridge_library.capabilities.publish import Publish
from rosbridge_library.capabilities.subscribe import Subscribe
from rosbridge_library.capabilities.advertise_service import AdvertiseService
from rosbridge_library.capabilities.unadvertise_service import UnadvertiseService
from rosbridge_library.capabilities.call_service import CallService
def shutdown_hook():
IOLoop.instance().stop()
if __name__ == "__main__":
rospy.init_node("rosbridge_websocket")
rospy.on_shutdown(
shutdown_hook) # register shutdown hook to stop the server
##################################################
# Parameter handling #
##################################################
retry_startup_delay = rospy.get_param('~retry_startup_delay',
2.0) # seconds
# get RosbridgeProtocol parameters
RosbridgeWebSocket.fragment_timeout = rospy.get_param(
'~fragment_timeout', RosbridgeWebSocket.fragment_timeout)
RosbridgeWebSocket.delay_between_messages = rospy.get_param(
'~delay_between_messages', RosbridgeWebSocket.delay_between_messages)
RosbridgeWebSocket.max_message_size = rospy.get_param(
In this case, we only need the orientation of the cube and the speed of the disc.
The distance doesn't condition at all the actions
"""
disk_roll_vel = observations[0]
# roll_angle = observations[2]
y_linear_speed = observations[4]
yaw_angle = observations[5]
state_converted = [disk_roll_vel, y_linear_speed, yaw_angle]
return state_converted
if __name__ == '__main__':
rospy.init_node('j2n6s300_gym', anonymous=True, log_level=rospy.WARN)
# Create the Gym environment
env = gym.make('j2n6s300Test-v3')
rospy.loginfo("Gym environment done")
# Set the logging system
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('j2n6s300_ml')
outdir = pkg_path + '/training_results'
env = wrappers.Monitor(env, outdir, force=True)
rospy.loginfo("Monitor Wrapper started")
last_time_steps = numpy.ndarray(0)
# Loads parameters from the ROS param server
#!/usr/bin/env python
import roslib; roslib.load_manifest('sample_tts')
import rospy, os, sys
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
from std_msgs.msg import String
def foundCallback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
s = soundhandle.voiceSound(data.data)
s.play()
rospy.sleep(1)
if __name__ == '__main__':
rospy.init_node('soundplay_test', anonymous=True)
soundhandle = SoundClient()
rospy.sleep(1)
rospy.Subscriber("sound_listener", String, foundCallback)
while not rospy.is_shutdown():
rospy.spin()
def main():
rospy.init_node("auto_nav_position", anonymous=False)
rate = rospy.Rate(20)
autoNav = AutoNav()
autoNav.distance = 4
last_detection = []
while not rospy.is_shutdown() and autoNav.activated:
rospy.loginfo("AutoNav is activated")
#rospy.loginfo(autoNav.objects_list)
rospy.loginfo(last_detection)
if autoNav.objects_list != last_detection:
rospy.loginfo("Last detection not activated")
if autoNav.state == -1:
rospy.loginfo("AutoNav.state == -1")
while (not rospy.is_shutdown()) and (len(autoNav.objects_list) < 3):
autoNav.test.publish(autoNav.state)
rospy.loginfo("AutoNav.state in -1")
rate.sleep()
autoNav.state = 0
# last_detection = autoNav.objects_list
if autoNav.state == 0:
rospy.loginfo("AutoNav.state == 0")
autoNav.test.publish(autoNav.state)
if len(autoNav.objects_list) >= 3:
rospy.loginfo("AutoNav.objects_list) >= 3")
autoNav.calculate_distance_to_sub()
if (len(autoNav.objects_list) >= 3) and (autoNav.distance >= 2):
rospy.loginfo("AutoNav.objects_list) >= 3 and (autoNav.distance >= 2)")
autoNav.center_point()
else:
rospy.loginfo("No autoNav.objects_list")
initTime = rospy.Time.now().secs
while ((not rospy.is_shutdown()) and
(len(autoNav.objects_list) < 3 or autoNav.distance < 2)):
rospy.loginfo("not rospy.is_shutdown() and (len(autoNav.objects_list) < 3 or autoNav.distance < 2)")
if rospy.Time.now().secs - initTime > 2:
rospy.loginfo("rospy.Time.now().secs - initTime > 2")
autoNav.state = 1
rate.sleep()
break
#last_detection = autoNav.objects_list
if autoNav.state == 1:
rospy.loginfo("AutoNav.state == 1")
autoNav.test.publish(autoNav.state)
if len(autoNav.objects_list) >= 3:
autoNav.state = 2
else:
initTime = rospy.Time.now().secs
while ((not rospy.is_shutdown()) and
(len(autoNav.objects_list) < 3)):
if rospy.Time.now().secs - initTime > 1:
autoNav.farther()
rate.sleep()
break
#last_detection = autoNav.objects_list
if autoNav.objects_list != last_detection:
rospy.loginfo("autoNav.objects_list != last_detection:")
if autoNav.state == 2:
rospy.loginfo("AutoNav.state == 2")
autoNav.test.publish(autoNav.state)
if len(autoNav.objects_list) >= 3:
autoNav.calculate_distance_to_sub()
if len(autoNav.objects_list) >= 3 and autoNav.distance >= 2:
autoNav.center_point()
else:
initTime = rospy.Time.now().secs
while ((not rospy.is_shutdown()) and
(len(autoNav.objects_list) < 3 or autoNav.distance < 2)):
if rospy.Time.now().secs - initTime > 2:
autoNav.state = 3
rate.sleep()
break
# last_detection = autoNav.objects_list
elif autoNav.state == 3:
autoNav.test.publish(autoNav.state)
time.sleep(1)
autoNav.status_pub.publish(1)
rate.sleep()
rospy.spin()
import Common.util
pub_aruco_3d = rospy.Publisher('/jevois/ArUco/3D',
Float32MultiArray,
queue_size=1)
def publish(poss):
if not rospy.is_shutdown():
if len(poss) == 6:
pub_aruco_3d.publish(data=poss)
stop = False
rospy.init_node('ArUco_3D')
def signal_handler(signal, frame):
print('^C')
global stop
stop = True
sys.exit()
signal.signal(signal.SIGINT, signal_handler)
N_FIT = 3
last_positions = zeros((N_FIT, 6))
last_times = zeros(N_FIT)
#!/usr/bin/env python
import time
import rospy
from std_msgs.msg import Bool, Time
import RPi.GPIO as GPIO
if __name__ == "__main__":
rospy.init_node('camera_trigger_node')
trigger_pub = rospy.Publisher('trigger', Time, queue_size=1)
trigger_pin = rospy.get_param('~trigger_pin', 26)
trigger_rate = rospy.get_param('~trigger_rate',
2) # in Hz. Defaults at 2Hz
GPIO.setmode(GPIO.BCM)
GPIO.setup(trigger_pin, GPIO.OUT)
r = rospy.Rate(trigger_rate) # hz
pulse_width = 0.05
while not rospy.is_shutdown():
# Write output high
GPIO.output(trigger_pin, GPIO.HIGH)
# Sleep
time.sleep(pulse_width)
# Write low
GPIO.output(trigger_pin, GPIO.LOW)
tmsg = Time()
tmsg.data = rospy.get_rostime()
trigger_pub.publish(tmsg)
r.sleep()
peri = cv2.arcLength(c, True) # calculates a contour curve length
c1 = cv2.convexHull(c)
approx = cv2.approxPolyDP(c1, 0.03 * peri, True) #Approximates a polygonal curve with curve length (define how curve would be a line)
# if the shape is a triangle, it will have 3 vertices
if len(approx) == 3:
shape = "triangle"
# if the shape has 4 vertices, it is either a square or a rectangle
elif len(approx) == 4:
# compute the bounding box of the contour and use the bounding box to compute the aspect ratio
(x, y, w, h) = cv2.boundingRect(approx)
ar = w / float(h)
# a square will have an aspect ratio that is approximately equal to one, otherwise, the shape is a rectangle
shape = "square" if ar >= 0.7 and ar <= 1.3 else "rectangle"
# if it's not as us expect, generalize it as unidentified
if h < 5 or w < 5:
shape = "unidentified"
# otherwise, we assume the shape is a circle
else:
shape = "circle"
# return the name of the shape
return shape
if __name__ == "__main__":
rospy.init_node("depth_detect_cir_node",anonymous=False)
depth_detect_cir_node = depth_detect_cir()
rospy.spin()
workbook = xlsxwriter.Workbook(waktu+'.xlsx')
worksheet = workbook.add_worksheet()
# Some data we want to write to the worksheet.
worksheet.write(1, 2, 'Sumbu X')
worksheet.write(1, 3, 'Sumbu Y')
worksheet.write(1, 4, 'Sumbu Z')
# Start from the first cell. Rows and columns are zero indexed.
row = 2
col = 2
if __name__ == '__main__':
rospy.init_node('dataexcel', anonymous=True)
rospy.Subscriber("/makarax/PosX", String, PosX)
rospy.Subscriber("/makarax/PosY", String, PosY)
rospy.Subscriber("/makarax/PosZ", String, PosZ)
rate = rospy.Rate(1)
PosisiX = None
PosisiY = None
PosisiZ = None
while not rospy.is_shutdown():
worksheet.write(row,col,PosisiX)
worksheet.write(row, col + 1, PosisiY)
worksheet.write(row, col + 2, PosisiZ)
row += 1
rate.sleep()
workbook.close()
#!/usr/bin/env python
from __future__ import division
import rospy
import tf
import cv2
import argparse
import sys
import numpy as np
rospy.init_node("tf_fudger", anonymous=True)
br = tf.TransformBroadcaster()
usage_msg = "Useful to test transforms between two frames."
desc_msg = "Pass the name of the parent frame and the child name as well as a an optional multiplier. \n\
At anypoint you can press q to switch between displaying a quaternion and an euler angle. \n\
Press 's' to exit and REPLACE or ADD the line in the tf launch file. Use control-c if you don't want to save."
parser = argparse.ArgumentParser(usage=usage_msg, description=desc_msg)
parser.add_argument(dest='tf_parent',
help="Parent tf frame.")
parser.add_argument(dest='tf_child',
help="Child tf frame.")
parser.add_argument('--range', action='store', type=float, default=3,
help="Maximum range allowed for linear displacement (in meters)")
args = parser.parse_args(sys.argv[1:])
cv2.namedWindow('tf', cv2.WINDOW_NORMAL)
# slider max values
def main():
rospy.init_node('person')
rospy.Subscriber("/person_detector/detections", Detections2d, callback)
rospy.spin()
####Message box
buffer=[]
def cb_sub(msg):
buffer.append(msg.data)
########################################################
def parse_argv(argv):
args={}
for arg in argv:
tokens = arg.split(":=")
if len(tokens) == 2:
key = tokens[0]
args[key] = tokens[1]
return args
########################################################
rospy.init_node("control_panel",anonymous=True)
Config.update(parse_argv(sys.argv))
try:
Config.update(rospy.get_param("~config"))
except Exception as e:
print "get_param exception:",e.args
#try:
# Param.update(rospy.get_param("~param"))
#except Exception as e:
# print "get_param exception:",e.args
####sub pub
rospy.Subscriber("~load",String,cb_load)
rospy.Subscriber("/message",String,cb_sub)
pub_Y3=rospy.Publisher("~loaded",Bool,queue_size=1)
pub_E3=rospy.Publisher("~failed",Bool,queue_size=1)
steeringAngle = self.maxSteeringAngle
if steeringAngle < -0.32:
steeringAngle = -0.32
return steeringAngle, Designated_Speed
steeringAngle = 0
Designated_Speed = 0
return steeringAngle, Designated_Speed
def publishCommand(self, velocity, angle):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "line_follower"
drivingCommand = AckermannDriveStamped()
drivingCommand.drive.speed = velocity
drivingCommand.drive.steering_angle = angle
print(drivingCommand)
self.commandPub.publish(drivingCommand)
if __name__ == "__main__":
rospy.init_node('conga_line')
node = Controller()
rospy.spin()
ar_tag = x
ar_tag_pos = ar_tag.pose.pose.position
x = ar_tag_pos.x
y = ar_tag_pos.z
speed = y - self.dist_des
steering_angle = -np.arctan2((x - 0.005), y)
self.calculate_turn(speed, steering_angle)
def calculate_turn(self, speed, steering_angle):
u_steer = steering_angle
saturation = 0.34
if abs(u_steer) > saturation:
u_steer = np.sign(u_steer) * saturation
z = self.K * speed
if abs(z) > self.speed_cap:
z = np.sign(z) * self.speed_cap
cmd_msg = AckermannDriveStamped()
cmd_msg.drive.steering_angle = u_steer
cmd_msg.drive.speed = z
cmd_msg.header.stamp = rospy.Time.now()
self.cmd_pub.publish(cmd_msg)
if __name__ == "__main__":
rospy.init_node("ar_control")
node = arController()
rospy.spin()
rospy.loginfo("Invalid input")
return -1
def print_motor_config(self, cmd):
if (cmd == "m"):
odrv.dump_motor_config()
return 0
elif (cmd == "e"):
odrv.dump_encoder_config()
return 0
else:
rospy.loginfo("Invalid input")
return -1
def print_error(self, cmd):
odrv.dump_errors()
return 0
def loop(self):
angle_position_val = convert_counts_to_angle(self.get_position())
self.PIDpub.publish(angle_position_val)
if __name__ == "__main__":
rospy.init_node("odriv_node", anonymous=True)
odrv = odrive_exo()
while (True):
odrv.loop()
rospy.spin()
if mode:
u_sail = data.data
publish[1] = 1
##############################################################################################
# Main
##############################################################################################
if __name__ == '__main__':
mode = 0 # 0=controler, 1=xbee
u_rudder, u_sail = 0, 0
publish = [0, 0] # publish rudder, publish sail
rospy.init_node('mode')
pub_send_u_rudder = rospy.Publisher('mode_send_u_rudder',
Float32,
queue_size=10)
pub_send_u_sail = rospy.Publisher('mode_send_u_sail',
Float32,
queue_size=10)
rospy.Subscriber("control_send_u_rudder", Float32, sub_control_u_rudder)
rospy.Subscriber("control_send_u_sail", Float32, sub_control_u_sail)
rospy.Subscriber("xbee_send_u_rudder", Float32, sub_xbee_u_rudder)
rospy.Subscriber("xbee_send_u_sail", Float32, sub_xbee_u_sail)
rospy.Subscriber("xbee_send_mode", Float32, sub_xbee_mode)
u_rudder_msg = Float32()
u_sail_msg = Float32()
def ping_response_server():
rospy.init_node('ping_response_server')
s = rospy.Service('ping_response', PingResponse, handle_ping)
print("Ready to respond to ping")
rospy.spin()
#Takes actual time to velocity calculus
t1=rospy.Time.now().to_sec()
#Calculates distancePoseStamped
current_distance= speed*(t1-t0)
#After the loop, stops the robot
vel_msg.linear.x = 0
#Force the robot to stop
self.velocity_publisher.publish(vel_msg)
if __name__ == '__main__':
# Starts a new node
# rospy.init_node('robot_cleaner', anonymous=True)
# velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
# distance_subscriber = rospy.Subscriber("/gps_dist", Float32, callback)
rospy.init_node('qc_foward', anonymous=True)
velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
'''
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
self.distance_subscriber = rospy.Subscriber("/gps_dist", Float32, self.callback)
self.float32 = Float32()
self.rate = rospy.Rate(10)
'''
try:
timeout = time.time() + 60
while time.time() < timeout:
x = Forward()
x.move(velocity_publisher)
time.sleep(30)
except rospy.ROSInterruptException:
rospy.logerr('Unable to open camera stream: ' + str(url))
sys.exit() #'Unable to open camera stream')
rospy.logerr('got the url!!! ')
self.bytes=''
self.image_pub = rospy.Publisher("camera_image", Image)
self.bridge = CvBridge()
if __name__ == '__main__':
parser = argparse.ArgumentParser(prog='ip_camera.py', description='reads a given url string and dumps it to a ros_image topic')
parser.add_argument('-g', '--gui', action='store_true', help='show a GUI of the camera stream')
#define your IP Camera URL in here. My Camera is using Belkin Camera, which is using this address to access the images
#or you can type in your command like this: $python ip_camera -u YOUR_CAMERA_URL -g
parser.add_argument('-u', '--url', default='http://10.42.0.40:8080/mjpegfeed?320x240', help='camera stream url to parse')
args = parser.parse_args()
rospy.init_node('IPCamera', anonymous=True)
ip_camera = IPCamera(args.url)
rospy.logerr('all set going into loop :)')
while not rospy.is_shutdown():
ip_camera.bytes += ip_camera.stream.read(1024)
a = ip_camera.bytes.find('\xff\xd8')
b = ip_camera.bytes.find('\xff\xd9')
if a!=-1 and b!=-1:
jpg = ip_camera.bytes[a:b+2]
ip_camera.bytes= ip_camera.bytes[b+2:]
i = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8),cv2.CV_LOAD_IMAGE_COLOR)
image_message = cv.fromarray(i)
cv2.imshow('video', i)
image_message = np.asarray(image_message[:,:])
ip_camera.image_pub.publish(ip_camera.bridge.cv2_to_imgmsg(image_message, "bgr8"))
#rospy.Subscriber('/object_color', String, self.callback_get_object_color)
def get_colors_range(self):
lower_blue = np.array([15, 000, 0])
upper_blue = np.array([17, 200, 200])
lower_red = np.array([80, 100, 100])
upper_red = np.array([255, 255, 255])
lower_yellow = np.array([20, 0, 0])
upper_yellow = np.array([40, 255, 255])
lower_green = np.array([40, 0, 0])
upper_green = np.array([80, 255, 255])
#lower_blue = np.array([80, 0, 0])
#upper_blue = np.array([130, 255, 255])
# define range of blue color in HSV
lower_purple = np.array([130, 0, 0])
upper_purple = np.array([255, 255, 255])
res = {'blue': (lower_blue, upper_blue), 'green': (lower_green, upper_green),
'red': (lower_red, upper_red), 'purple': (lower_purple, upper_purple), 'yellow': (lower_yellow, upper_yellow)}
return res
if __name__ == '__main__':
rospy.init_node('pub_angles_from_colored_obj', anonymous=True)
pub_angles_from_colored_obj()
rospy.spin()
def vision_node():
rospy.init_node("endeffector")
s = rospy.Service("move_endeffector", MoveEndEffector, handle_move_endeffector)
rospy.spin()
def main():
rospy.init_node("pa2_poseestimator")
pose_estimator = PoseEstimator()
pose_estimator.spin()