def __init__(self):
rospy.init_node('Robot Tracking')
# TCP_IP = rospy.get_param('~IP', 'localhost')
# TCP_PORT = rospy.get_param('~PORT', 6666)
# self.BUFFER_SIZE = 1024
# self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# self.s.connect((TCP_IP, TCP_PORT))
self.validId = Int8MultiArray()
self.f = open(
'/home/abhay/ROS_Projects/test/src/testpkg/scripts/data.txt', 'r')
# Subsciber_generator.subscribe('/CT0/validity', Int8MultiArray, self.ct0_valid_callback)
rospy.Subscriber('/CT0/validity', Int8MultiArray,
self.ct0_valid_callback)
self.location_pub = rospy.Publisher('~poses', String, queue_size=10)
self.valid_id_pub = rospy.Publisher('~validity',
Int8MultiArray,
queue_size=10)
self.rate = rospy.get_param('~rate', 10)
self.test = Int8MultiArray()
self.validity_ct0 = []
self.valid_id_count_ct0 = 0
def publish_arming_Message(self):
if (self.arming_flag):
vehicle_status_array = Int8MultiArray()
vehicle_status_array.data = [1]
self.confirm_flag_pub.publish(vehicle_status_array)
else:
vehicle_status_array = Int8MultiArray()
vehicle_status_array.data = [0]
self.confirm_flag_pub.publish(vehicle_status_array)
def main():
game_pad = GamePad(0)
game_pad.button_mode_[0] = 1
rospy.init_node('GamePad', anonymous=True, disable_signals=True)
pub = rospy.Publisher('GamePad', Int8MultiArray, queue_size=30)
rospy.on_shutdown(shutdown)
rate = rospy.Rate(5)
# button_frag = False
while True:
game_pad.Update()
# if game_pad.buttons_[0] == True:
# button_frag = not button_frag
game_pad_state = Int8MultiArray()
game_pad_state.data = np.zeros(len(game_pad.buttons_))
print game_pad_state.data
# game_pad_state.data = [int(button_frag)] + game_pad.axes_[:3] + list(game_pad.hats_[0])
# print game_pad_state.data
for i in range(len(game_pad.buttons_)):
game_pad_state.data[i] = int(game_pad.buttons_[i])
print game_pad_state
pub.publish(game_pad_state)
rate.sleep()
rospy.spin()
def image_process(self):
print("Processing Image")
# Resize the photo and apply Canny Edge Detector
# Generate edgemap and send to display
edges = self.resize_edge(self.face, 80)
# Save locally
cv2.imwrite("/home/ethan/Pictures/face.jpg", self.face)
cv2.imwrite("/home/ethan/Pictures/edge.jpg", edges)
# Resize the edgemap image
edges_resize = self.resize(edges, 600)
edges_rgb = cv2.cvtColor(edges_resize, cv2.COLOR_GRAY2RGB)
# Add Jarvis as background
self.jarvis[0:edges_rgb.shape[0], 0:edges_rgb.shape[1]] = edges_rgb
edges_ros = self.bridge.cv2_to_imgmsg(self.jarvis,
encoding="passthrough")
self.image_pub.publish(edges_ros)
print("Sending edges to display")
# Apply DFS to transfer binary image to trajectory points
pathlist = getPointsFromEdges(edges)
# (-1,-1) to lift up the end effector
pathlist.append((-1, -1))
print("Path length: ", len(pathlist))
# Publish to drawing node
path = Int8MultiArray()
pathlist = np.array(pathlist).reshape(2 * len(pathlist))
path.data = pathlist
self.trajectory_pub.publish(path)
print("Publishing Trajectory")
def player():
pub = rospy.Publisher('mecanum_motors', Int8MultiArray, queue_size=10)
rospy.init_node('player', anonymous=True)
r = rospy.Rate(10) # 10hz
loadFile(fileName)
global i
global lines
# print lines
print len(lines)
print i
while not rospy.is_shutdown():
if lines:
if i < len(lines):
print 'odom_run: ',
line = lines[i]
print line
arr = []
for j in range(0, 4):
arr.append(int(line[j]))
pub.publish(Int8MultiArray(data=arr))
i += 1
elif i == len(lines):
#finish
task_pub.publish(Int8(Tasks.ODOMRUN_END))
i += 1
else:
pass
#str = "hello world %s"%rospy.get_time()
#rospy.loginfo(str)
#pub.publish(str)
r.sleep()
def pubby(outlist):
# while not rospy.is_shutdown():
stripe_obs_data = Int8MultiArray()
#Need Function that runs algorithm and returns outlist
stripe_obs_data.data = outlist
#rospy.loginfo(stripe_obs_data)
pub.publish(stripe_obs_data)
def _record_demonstration(self):
observations = []
while True:
robot = self.environment.robot
if robot._gripper:
if robot._cuff.upper_button():
robot._gripper.open()
elif robot._cuff.lower_button():
robot._gripper.close()
data = {
"time": self._time_stamp(),
"robot": self.environment.get_robot_state(),
"items": self.environment.get_item_state(),
"triggered_constraints": self.environment.check_constraint_triggers()
}
observation = Observation(data)
if self.publish_constraint_validity:
valid_constraints = check_constraint_validity(self.environment, self.environment.constraints, observation)[1]
pub = rospy.Publisher('cairo_lfd/valid_constraints', Int8MultiArray, queue_size=10)
msg = Int8MultiArray(data=valid_constraints)
pub.publish(msg)
observations.append(observation)
if self.command == "discard":
rospy.loginfo("~~~DISCARDED~~~")
self.interaction_publisher.send_position_mode_cmd()
self._clear_command()
return []
if self.command == "capture":
rospy.loginfo("~~~CAPTURED~~~")
self.interaction_publisher.send_position_mode_cmd()
self._clear_command()
rospy.loginfo("{} observations captured.".format(len(observations)))
return observations
self._rate.sleep()
def __init__(self):
self._nodeCost = rclpy.create_node('costmap')
self.pubCost = self._nodeCost.create_publisher(Int8MultiArray,
'/yf_camera/costmap', 1)
self.pubCost # prevent unused variable warning
self._cost = Int8MultiArray()
def somethingCool():
global mapdata
mapdata = Int8MultiArray()
rospy.init_node('test_name', anonymous=False)
rospy.Subscriber("map", OccupancyGrid, callback)
rospy.loginfo(mapdata)
rospy.spin()
def talker():
data_list = Int8MultiArray()
pub = rospy.Publisher('Testmessage', Int8MultiArray, queue_size=1)
rospy.init_node('talker', anonymous=True)
while not rospy.is_shutdown():
data_list.data = [50, 50]
pub.publish(data_list)
time.sleep(1)
def callback(self, heat_array):
coordinates = get_bright_loc(heat_array.data)
self.get_logger().info(f'Coordinates: X:{coordinates[0]}, Y:{coordinates[1]}')
comm_array = Int8MultiArray()
comm_array.data = command(coordinates)
self.publisher_.publish(comm_array)
self.get_logger().info(
f'Direction: X:{comm_array.data[0]}, Y:{comm_array.data[1]}, Active:{comm_array.data[2]}')
def talker():
data_list = Int8MultiArray()
pub = rospy.Publisher('contl_data', Int8MultiArray, queue_size=2)
rospy.init_node('talker', anonymous=True)
while not rospy.is_shutdown():
data_list.data = [5, 5]
pub.publish(data_list)
time.sleep(1)
def pubLabelMsg(self, known_labels):
msg = Int8MultiArray()
dim = MultiArrayDimension()
dim.label = "length"
dim.size = len(known_labels)
dim.stride = len(known_labels)
msg.layout.dim.append(dim)
msg.layout.data_offset = 0
msg.data = known_labels
self.labels_pub.publish(msg)
def on_move_emitted(client, userdata, message):
UNUSED(client)
UNUSED(userdata)
msg = str(message.payload.decode("utf-8"))
values = np.array(msg.split(','), dtype=np.int8)
assert values.shape == (3, )
state = Int8MultiArray()
state.data = list(values)
MOVE_ROS_PUBLISHER.publish(state)
print('MOVE with shape {} has values: {}'.format(values.shape, msg))
def on_board_state_changed(client, userdata, message):
UNUSED(client)
UNUSED(userdata)
msg = str(message.payload.decode("utf-8"))
values = np.array(msg.split(','), dtype=np.int8)
print('RECEIVED: ', msg)
assert values.shape == (9, )
state = Int8MultiArray()
state.data = list(values)
BOARD_STATE_ROS_PUBLISHER.publish(state)
print('BOARD STATE with shape {} has values: {}'.format(values.shape, msg))
def listener():
pub = rospy.Publisher('contl_data',Int8MultiArray,queue_size=1)
rospy.init_node('listener', anonymous=True)
data_list = Int8MultiArray()
global stealing
global throttle
while not rospy.is_shutdown():
data_list.data = [int(throttle),int(stealing)]
print(data_list.data)
pub.publish(data_list)
time.sleep(0.01)
rospy.Subscriber("joy",Joy,callback)
def talker():
#############################################
#
pub=rospy.Publisher('/servo',UInt16, queue_size=10)
pub2=rospy.Publisher('/servo2',UInt16, queue_size=10)
# values between 30 and 150
# 30 50 70 90 110 130 150
selected_column=2
arm_position = UInt16()
arm_position.data = 30+20*selected_column
pub.publish(arm_position)
open_hand = UInt16()
open_hand.data= 50
pub2.publish(open_hand)
#############################################
pub = rospy.Publisher('new_action', Int8MultiArray, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(0.2) # publish every 1/rate seconds
while not rospy.is_shutdown():
new_action = Int8MultiArray()
new_action.layout.dim = []
new_action.data=[1,1,2,3]
# 1st cell: 0 -> cv; 1 -> talk; 2 -> logic
# 2nd cell: 1 -> player; 2 -> Roboy
# 3rd cell: row-coordinate
# 4th cell: column-coordinate
pub.publish(new_action)
rate.sleep()
def talker(size):
'''This is the main function of the whole script. Here we set up all of
publishers and subscribers. In short, subscribe to the map and attach it
to genTunnel (via callback()). explorer is updated in the process and we
can then call the navigation engine functions on it. A path is returned
and the next waypoint is published on the /next_wp topic'''
global pubway, rtl_pub
mapdata = Int8MultiArray()
rospy.init_node('talker', anonymous=True)
pubway = rospy.Publisher('next_wps', Waypoints, queue_size=10)
rtl_pub = rospy.Publisher('return_to_launch', Bool, queue_size=10)
rtl_pub.publish(False)
rospy.Subscriber("nav_map", OccupancyGrid, occ_grid_cb)
rospy.spin()
def main(self):
rate = rospy.Rate(40) # Hz
while not rospy.is_shutdown():
#if self.state is not None:
# msg = Bool()
# msg.data=self.state
# self.publisher.publish(msg)
msg = Int8MultiArray()
msg.data = [
self.state[0], self.state[1], self.state[2], self.state[3]
] #,self.state[4],self.state[5]]
self.publisher.publish(msg)
rate.sleep()
def joy_cb(msg):
throttle = -1
if msg.axes[axes['LY']] > 0:
throttle += msg.axes[axes['LY']] * 2
rudder = msg.axes[axes['LX']]
elevator = msg.axes[axes['RY']]
aileron = msg.axes[axes['RX']]
pub_msg = Int8MultiArray()
factor = 127
pub_msg.data.append(throttle * factor)
pub_msg.data.append(rudder * factor)
pub_msg.data.append(elevator * factor)
pub_msg.data.append(aileron * factor)
pub_msg.data.append(-aileron * factor)
rc_plane_cmd_Publisher.publish(pub_msg)
def callback(self, data, joy_data):
global stealing
global throttle
global flag
data_list = Int8MultiArray()
if (data.buttons[3] == 1): #start=>linetrace
flag = 1
elif (data.buttons[0] == 1): #select=>ps3control
flag = 0
if (flag == 0):
data_ste = [round((data.axes[0] - 1) * -50, 1)] #round is 四捨五入
stealing = np.clip(data_ste, 35, 65)[0]
data_thr = [round((data.axes[1] - 1) * -50, 1)]
throttle = np.clip(data_thr, 35, 65)[0]
elif (flag == 1):
throttle = 45
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
gray_image = cv2.cvtColor(cv_image[105:119, 0:159],
cv2.COLOR_BGR2GRAY)
ret, cv_image2 = cv2.threshold(gray_image, 150, 255,
cv2.THRESH_BINARY_INV)
thresh = 210
max_pixel = 255
ret, img_dst = cv2.threshold(gray_image, thresh, max_pixel,
cv2.THRESH_BINARY)
img_binary = img_dst / 255
line_data = np.sum(img_binary, axis=0)
line_data_left = line_data[0:((line_data.shape[0] / 2) - 1)].sum()
line_data_right = line_data[(line_data.shape[0] /
2):(line_data.shape[0] - 1)].sum()
cont_data = [float(line_data_left) - float(line_data_right)]
cont_data = np.clip(cont_data, -50, 50)[0]
cont_data = (cont_data - 50) * -1
print(cont_data)
stealing = cont_data
cv2.imshow("img_dst", img_dst)
cv2.waitKey(2)
data_list.data = [int(throttle), int(stealing)]
self.image_pub.publish(data_list)
def parking_possiblity(
ros_data
): ### using web_cam, check white_blob and line and enough distance, if all exist, it send Availablity
global white_detected
global park_enable
global d
if stage is 1:
np_arr = np.fromstring(ros_data.data, np.uint8) ### image process
frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
mask_white = cv2.inRange(hsv, lower_white, upper_white)
res1 = cv2.bitwise_and(frame, frame, mask=mask_white)
line = turtle_video_siljun.find_line(
frame) ### Checking if there are lines
keypoints = turtle_video_siljun.find_white(
frame, lower_white, upper_white) ### Checking if there are white
if line > 0 and keypoints:
white_detected = 1 ### check white line
else:
white_detected = 0
n = 1
for i in d:
if i > 0.12 and i < 0.6:
n = 0 ### check parking space
if n == 0:
park_enable = 0
else:
park_enable = 1
cv2.imshow('frac', frame) ### show processd image
cv2.imshow('white', res1)
cv2.waitKey(1) & 0xFF
print(white_detected)
state = Int8MultiArray()
state.data = [white_detected, park_enable]
pub1.publish(state) ### pub white_detected and park_enable to 'main'
def talker(size):
'''This is the main function of the whole script. Here we set up all of
publishers and subscribers. In short, subscribe to the map and attach it
to genTunnel (via callback()). explorer is updated in the process and we
can then call the navigation engine functions on it. A path is returned
and the next waypoint is published on the /next_wp topic'''
global pubway
mapdata = Int8MultiArray()
rospy.init_node('talker', anonymous=True)
pub = rospy.Publisher('mapprob', Int8MultiArray, queue_size=10)
pubway = rospy.Publisher('next_wps', Waypoints, queue_size=10)
rospy.Subscriber("nav_map", OccupancyGrid, occ_grid_cb)
rospy.spin()
# time.sleep(10)
#shortest distance to consider and maximum range to use for navigation
#these work for a 30 x 30 but for a larger obstacle map may need to be
#larger. Some experimentation will be needed to find a good balance of
#performance
# shortestdistance = 3
# maxrange = 40
#get first move
# p = explorer.findClosestFrontier(size/2,size/2,maxrange,shortestdistance)
# last = False
#while we still have moves and have not returned to home base navigate
#around
# while p is not None:
# for pp in p:
# #jva.awt.point uses floats so we cast back to integers here
# way = Point()
# x = int(pp.getX())
# y = int(pp.getY())
# way.x = x
# way.y = y
# n = explorer.getNode(x,y)
# while not rospy.is_shutdown():
# pubway.publish(way)
#x_path,y_path = zip(*[(pp.getX(),pp,getY()) for pp in p])
'''ways = PoseArray()
def callback(self, data):
global stealing
global throttle
data_list = Int8MultiArray()
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#RGB表色系からHSV表色系に変換
#hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
#hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
#color_min = np.array([150,100,150])
#color_max = np.array([180,255,255])
#color_mask = cv2.inRange(hsv_image, color_min, color_max)
#cv_image2 = cv2.bitwise_and(cv_image, cv_image, mask = color_mask)
gray_image = cv2.cvtColor(cv_image[105:119, 0:159], cv2.COLOR_BGR2GRAY)
#histr = cv2.calcHist([gray_image],[0],None,[256],[0,256])
#plt.plot(histr,color = 'k')
#plt.show()
ret, cv_image2 = cv2.threshold(gray_image, 150, 255,
cv2.THRESH_BINARY_INV)
thresh = 210
max_pixel = 255
ret, img_dst = cv2.threshold(gray_image, thresh, max_pixel,
cv2.THRESH_BINARY)
img_binary = img_dst / 255
line_data = np.sum(img_binary, axis=0)
line_data_left = line_data[0:((line_data.shape[0] / 2) - 1)].sum()
line_data_right = line_data[(line_data.shape[0] /
2):(line_data.shape[0] - 1)].sum()
#print("left :{} right:{}".format(line_data_left,line_data_right))
cont_data = [float(line_data_left) - float(line_data_right)]
#print(cont_data)
cont_data = np.clip(cont_data, -50, 50)[0]
cont_data = (cont_data - 50) * -1
print(cont_data)
stealing = cont_data
data_list.data = [int(throttle), int(stealing)]
self.image_pub.publish(data_list)
cv2.imshow("img_dst", img_dst)
cv2.waitKey(2)
def talker():
pub = rospy.Publisher('new_action', Int8MultiArray, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(0.2) # publish every 1/rate seconds
while not rospy.is_shutdown():
new_action = Int8MultiArray()
new_action.layout.dim = []
new_action.data = [1, 1, 2, 3]
# 1st cell: 0 -> cv; 1 -> talk; 2 -> logic
# 2nd cell: 1 -> player; 2 -> Roboy
# 3rd cell: row-coordinate
# 4th cell: column-coordinate
pub.publish(new_action)
rate.sleep()
def update_temperature(self):
t0 = time.time()
temperatures = []
for i in [
(self.thermometer_range_m, 0), (0, -self.thermometer_range_m),
(-self.thermometer_range_m, 0), (0, self.thermometer_range_m)
]:
robot_angle = getHeading(self.pose.orientation)
sensor_pos_x = self.pose.position.x + i[0] * math.cos(
robot_angle) - i[1] * math.sin(robot_angle)
sensor_pos_y = self.pose.position.y + i[1] * math.cos(
robot_angle) + i[0] * math.sin(robot_angle)
pixel_nb = self.converter.RealToOccupancyGrid(
sensor_pos_x, sensor_pos_y)
temperatures.append(self.firemap.data[pixel_nb])
array = Int8MultiArray()
array.data = temperatures
self.pub.publish(array)
print(temperatures)
def shinho(blob_ROI, stage, angular): ###Function that run when stage=0
global f_r
global s_g
global sinho_state
sinho_state = Int8MultiArray()
if f_g == 1 and f_r == 0 and s_g == 0:
keypoints_red = turtle_video_siljun.find_color(blob_ROI, lower_red,
upper_red, stage)
print('first green signal detected.')
if keypoints_red:
f_r = 1
sinho_state.data = np.array([1, 0, 1])
pub_sinho.publish(sinho_state)
else:
sinho_state.data = np.array([0, 0, 0])
pub_sinho.publish(sinho_state)
return 0
if f_g == 1 and f_r == 1 and s_g == 0:
keypoints_green = turtle_video_siljun.find_color(
blob_ROI, lower_green, upper_green, stage)
print('red signal detected. waiting secondary green signal.')
sinho_state.data = np.array([1, 0, 0])
pub_sinho.publish(sinho_state)
if keypoints_green:
s_g = 1
sinho_state.data = np.array([1, 1, 1])
pub_sinho.publish(sinho_state)
return 0
if f_g == 1 and f_r == 1 and s_g == 1:
print('second green signal detected.')
s_g = 2
sinho_state.data = np.array([1, 2, 0])
pub_sinho.publish(sinho_state)
return 100
def track_motion_during_duration(counter_in):
State_pub = rospy.Publisher('SM/current_state',
Int8MultiArray,
queue_size=10)
State_msg = Int8MultiArray()
cmd_idx = counter_in
original_cmd_idx = cmd_idx
print "cmd_idx", cmd_idx
start_time = rospy.get_time()
print "start_time", start_time
duration = 0
iterator = 0
while duration < 10.0:
iterator = iterator + 1
action_state = generate_send_goal(cmd_idx)
curr_time = rospy.get_time()
# print "curr_time", curr_time
duration = curr_time - start_time
# print duration
if iterator % 100000 == 1:
print "duration time: %s, action_state %s," % (duration,
action_state)
if action_state == GoalStatus.SUCCEEDED:
cmd_idx = -1
iterator = 0
battery_msg = rospy.wait_for_message('/hsrb/battery_state', BatteryState)
State_msg.data.append(battery_msg.power)
State_msg.data.append(int(desired_states[original_cmd_idx]))
State_pub.publish(State_msg)
return action_state
def __init__(self):
rospy.init_node("controller", anonymous=False)
self._sub = rospy.Subscriber("joy", Joy, self.callback)
self._rate = rospy.Rate(50)
self._pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
self._velocity = Twist()
self._left_stick_y = 0.0
self._left_stick_x = 0.0
self._right_stick_x = 0.0
self._r2 = 0.0
self._l2 = 0.0
self._r22 = 0.0
self._l22 = 0.0
self._profile_2_linear_axis = 0.0
self._linear_speed_limit = rospy.get_param("teleop/linear_speed_limit", 1.50)
self._linear_speed_step = rospy.get_param("teleop/linear_speed_step", 0.10)
self._angular_speed_limit = rospy.get_param("teleop/angular_speed_limit", 1.00)
self._angular_speed_step = rospy.get_param("teleop/angular_speed_step", 0.05)
self._max_linear_speed = 1.0
self._max_angular_speed = 1.0
self._dpad_y = 0.0
self._dpad_x = 0.0
self._inverted_right_stick_x = 1 # for profile 1
self._inverted_left_stick_x = 1 # for profile 2
self._r3 = 1
self._l3 = 1
self._share_button = 0
self._cross = 0
self._profile = 1
self._linear_lock = False
self._inverted_angular_lock = False
self._angular_lock = False
self._share_lock = False
self._profile_2_linear_lock = True
# Remove in case of Quadrature encoder
self._sign_pub = rospy.Publisher("sign", Int8MultiArray, queue_size=1)
self._l_sign = 1
self._r_sign = 1
self._signs = Int8MultiArray()
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int8MultiArray
from std_msgs.msg import MultiArrayDimension
BUTTONS_NUM = 11
vel_msg = Twist()
button = Int8MultiArray()
button.data = [0 for _ in range(BUTTONS_NUM)]
rospy.init_node('joy_converter')
x_gain = rospy.get_param('~x', 1)
y_gain = rospy.get_param('~y', 1)
z_gain = rospy.get_param('~z', 1)
def callback(_data):
vel_msg.linear.x = _data.axes[1] * x_gain
vel_msg.linear.y = _data.axes[0] * y_gain
vel_msg.angular.z = _data.axes[3] * z_gain
for i in range(BUTTONS_NUM):
button.data[i] = _data.buttons[i]
def main():
pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
pub_buttons = rospy.Publisher('/buttons', Int8MultiArray, queue_size=10)
r = rospy.Rate(10)