def statusInterpreter(status):
# Variable Initialization
gripperStatus = 0
gripperMovement = 0
# Gripper Reset
if (status.gACT == 0) and (status.gIMC == 0):
gripperStatus = 0
# Gripper Activating
if (status.gACT == 1) and (status.gIMC == 1) and (status.gSTA == 0):
gripperStatus = 1
# Gripper Activated
if (status.gACT == 1) and (status.gIMC == 3):
gripperStatus = 2
# Gripper Open
if (status.gPOA < 110) and (status.gPOB < 110) and (status.gPOC < 110):
handClosed = False
# Gripper Closed
if (status.gPOA >= 110) and (status.gPOB >= 110) and (status.gPOC >= 110):
handClosed = True
# Gripper Moving
if (status.gDTA == 0) and (status.gDTA == 0) and (status.gDTA == 0):
handMoving = True
# Gripper Stopped
if (status.gDTA == 3) and (status.gDTA == 3) and (status.gDTA == 3):
handMoving = False
# Hand Moving and Opening
if handMoving and not handClosed:
gripperMovement = 0
# Hand Moving and Closing
if handMoving and handClosed:
gripperMovement = 1
# Hand Stoppend and Open
if not handMoving and not handClosed:
gripperMovement = 2
# Hand Stopped and Closed
if not handMoving and handClosed:
gripperMovement = 3
output = HoldingRegister()
output.data = [gripperStatus, gripperMovement]
# rospy.loginfo("Updating Robot Registers")
pub_robot.publish(output)
def setValues(self, address, value):
"""
Sets the requested values to the holding registers
and publishes they new values on a rostopic
:param address: The starting address
:type address: int
:param values: The new values to be set
:type values: list[int]
"""
# print "address",address,"value",value
ModbusSequentialDataBlock.setValues(self,address, value)
if address >= self.reading_start:
msg = HoldingRegister()
msg.data = value
self.pub.publish(msg)
def __init__(self):
rospy.init_node('global_path_viz')
#Publisher
self.global_path_marker_pub = rospy.Publisher('/global_path/path/viz', MarkerArray, queue_size=1, latch=True)
#Subscriber
self.node_edge_map_sub = rospy.Subscriber('/node_edge_map/map', NodeEdgeMap, self.node_edge_map_callback)
self.global_path_sub = rospy.Subscriber('/global_path/path', Int32MultiArray, self.global_path_callback)
self.global_path_marker = MarkerArray()
self.global_path = Int32MultiArray()
self.node_edge_map = NodeEdgeMap()
self.sub_map = False
def sensor_node():
c = collect_data()
pub = rospy.Publisher('/sensor_values', Int32MultiArray, queue_size=1)
rospy.init_node('sensor_node')
rate = rospy.Rate(50)
while not rospy.is_shutdown():
values = next(c)
msg = Int32MultiArray(
MultiArrayLayout(
[MultiArrayDimension('sensor data', 6, 1)],
1), # CHANGE the second arg to no. of sensor values read
values) # from serialport(just the values)
pub.publish(msg)
rate.sleep()
def talk_goal_pos():
global goalpos
rospy.init_node('goal_detector', anonymous=True)
pub = rospy.Publisher("goal_pos", Int32MultiArray, queue_size=10)
rospy.Subscriber("VideoRaw", Image, callback)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if goalpos == None:
pass
else:
goal_positions = Int32MultiArray(data=goalpos)
rospy.loginfo(goal_positions)
pub.publish(goal_positions)
rate.sleep()
def recive_frame_and_track(self, msg):
if self.init_rect==None:
return
if True:
cv2_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
if self.init_rect is not None and self.tracker_manager is None:
self.tracker_manager = tracker_manager(self.init_rect, cv2_img, self.regressor, self.tracker)
return
else:
bbox = self.tracker_manager.track_frame(cv2_img)
bbox_msg = Int32MultiArray()
bbox_msg.data = [int(bbox.x1), int(bbox.y1), int(bbox.x2), int(bbox.y2)]
print(bbox_msg)
self.goturn_pub.publish(bbox_msg)
def Sub_Red(self, msg):
if (msg is not None):
scan = [999] * SCAN_NUM
for i in range(len(msg.ranges)):
if (msg.ranges[i] == math.inf):
scan[i] = SCAN_LIMIT
elif (msg.ranges[i] == math.isnan):
scan[i] = 20
else:
scan[i] = int(round(msg.ranges[i], 2) * 100)
pub_msg = Int32MultiArray()
pub_msg.data = scan
self.pub_red.publish(pub_msg)
def callback(self, msg):
data = msg.data
numOfFaces = data[1]
frameRate = data[0]
pictureWidth = data[2]
pictureHeight = data[3]
filteredData = []
filteredData.append(data[0]) #frame rate
filteredData.append(0) #number of faces
filteredData.append(data[2]) #width
filteredData.append(data[3]) #height
faceCounter = 0
for i in range(numOfFaces):
if data[i * 6 + 5] > frameRate / 2 and data[
i * 6 + 8] > pictureWidth / 15 and data[
i * 6 + 9] > pictureHeight / 15:
filteredData.append(data[6 * i + 4]) #face index
filteredData.append(data[6 * i + 5]) #face duration
filteredData.append(data[6 * i + 6]) #face corner x
filteredData.append(data[6 * i + 7]) #face corner y
filteredData.append(data[6 * i + 8]) #face width
filteredData.append(data[6 * i + 9]) #face height
faceCounter += 1
#print filteredData
filteredData[1] = faceCounter
dataForPublish = Int32MultiArray(data=filteredData)
self.faceCoord_pub.publish(dataForPublish)
if not (self.SentTrigger) and faceCounter > 0:
self.facePresentCounter += 1
if self.SentTrigger and faceCounter == 0:
self.facePresentCounter += 1
if self.facePresentCounter > 2 * frameRate and not (self.SentTrigger):
self.facePresent_pub.publish(True)
self.SentTrigger = True
self.facePresentCounter = 0
if self.facePresentCounter > 6 * frameRate and self.SentTrigger:
self.facePresent_pub.publish(False)
self.SentTrigger = False
self.facePresentCounter = 0
def callback(self, data):
cv_image = self._bridge.imgmsg_to_cv2(data, 'bgr8')
gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
gray_image = gray_image > np.max(np.max(gray_image))
line1_data = gray_image[120:122, 1:640]
line2_data = gray_image[240:242, 1:640]
line3_data = gray_image[1:480, 480:482]
line4_data = gray_image[1:480, 320:322]
line1 = Int32MultiArray()
line2 = Int32MultiArray()
line3 = Int32MultiArray()
line4 = Int32MultiArray()
line1.data = np.sum(line1_data, axis=0)
line2.data = np.sum(line2_data, axis=0)
line3.data = np.sum(line3_data, axis=0)
line4.data = np.sum(line4_data, axis=0)
# tmpline = np.asarray(line1.data[1:638],dtype=float)-np.asarray(line1.data[2:639],dtype=float)
# inds = indices(np.abs(tmpline),lambda x: x > 20)
print np.array(line1.data, dtype=float) > np.max(line1.data) * 0.8
gray_image[120:122, 1:640] = 255
gray_image[240:242, 1:640] = 255
gray_image[1:480, 480:482] = 255
gray_image[1:480, 320:322] = 255
self._gray_pub.publish(self._bridge.cv2_to_imgmsg(gray_image, 'mono8'))
self._line1_pub.publish(line1)
self._line2_pub.publish(line2)
self._line3_pub.publish(line3)
self._line4_pub.publish(line4)
self._csteeringline.data = 0.0
self._csteeringline_pub.publish(self._csteeringline)
self._cstopline.data = False
self._cstopline_pub.publish(self._cstopline)
def finding_path(ros_data):
global flag
global final_path
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
frame = image_np
#cv2.imwrite('/home/benlee/catkin_ws/src/find_path/script/map.png',frame, params=[cv2.IMWRITE_PNG_COMPRESSION,0])
#frame = cv2.imread('/home/benlee/catkin_ws/src/find_path/script/map.png',cv2.IMREAD_COLOR)
start_point = Img_process.Basic_map()
end_point = Img_process.Basic_map()
obstacle_map = Img_process.Basic_map()
ret_start,start_x, start_y, start_img, s_w, s_h = start_point.find(frame, 'BLUE')
ret_end, end_x, end_y, end_img, e_w, e_h = end_point.find(frame, 'RED')
#print ("start",ret_start,"end",ret_end)
capstone_map = obstacle_map.obstacle_load(frame)
cv2.imshow('end_img', end_img)
cv2.imshow('img', frame)
cv2.imshow('start_img', start_img)
cv2.imshow('obstacle', capstone_map)
if ret_start == 1 and ret_end == 1:
start = (int(start_x), int(start_y))
end = (int(end_x), int(end_y))
# print("start:", start)
# print("end", end)
if flag == 0:
path = astar(capstone_map, start, end)
final_path = Int32MultiArray(data = path)
pub.publish(final_path)
#print ("path", path)
if path is not None and flag == 0:
if len(path)>0:
for i in range(0, len(path), 2):
# print ("i",i)
# print("path:", (path[i],path[i+1]))
cv2.circle(frame, (path[i],path[i+1]), 1, (0,0,255), 1)
flag = 1
cv2.imwrite('/home/benlee/catkin_ws/src/find_path/script/path.png',frame, params=[cv2.IMWRITE_PNG_COMPRESSION,0])
else:
print("no path bro")
else:
print(" no image, setting random start,end poinst")
found_path = cv2.imread('/home/benlee/catkin_ws/src/find_path/script/path.png',cv2.IMREAD_COLOR)
flag = 0
cv2.imshow('find path',found_path)
cv2.waitKey(1) & 0xFF
def talker():
pub = rospy.Publisher('cmd_joints', Int32MultiArray, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(40) # 10hz
i = 0
while not rospy.is_shutdown():
positions = Int32MultiArray()
positions.data = [
abs(int(1023 * sin(i))), 160, 800,
abs(int(800 * sin(i))), 500
]
i = i + 0.01
rospy.loginfo(positions)
pub.publish(positions)
rate.sleep()
def socket_talker(): ##創建Server node
pub = rospy.Publisher('chatter', Int32MultiArray, queue_size=10)
rospy.init_node(NAME)
#a = rospy.Service('arm_mode',arm_mode, Arm_Mode) ##server arm mode data
#s = rospy.Service('arm_pos',arm_data, point_data) ##server arm point data
#b = rospy.Service('speed_mode',speed_mode, Speed_Mode) ##server speed mode data
rate = rospy.Rate(100) # 10hz
print("Ready to connect")
while not rospy.is_shutdown():
# hello_str = "hello world %s" % rospy.get_time()
state = Int32MultiArray()
state.data = [state_feedback.ArmState, state_feedback.SentFlag]
pub.publish(state)
rate.sleep()
def _boxes_pub(self, boxes_in, scores_in, classes_in, width, height):
ROI_msgs = []
i = 0
while i < len(boxes_in[0]):
if scores_in[0][i] > 0.65:
min_x = boxes_in[0][i][1] * 640
max_x = boxes_in[0][i][3] * 640
min_y = boxes_in[0][i][0] * 480
max_y = boxes_in[0][i][2] * 480
ROI = [classes_in[0][i], min_x, max_x, min_y, max_y]
ROI_msgs = ROI_msgs + ROI
i = i + 1
print(ROI_msgs)
image_ROI = Int32MultiArray(data=ROI_msgs)
self._pub_ROI.publish(image_ROI)
def box_publish(boxes): msg = Int32MultiArray() msg.layout.dim = [MultiArrayDimension(),MultiArrayDimension()] n = 7 #Gandalfs number msg.layout.dim[0].label = "n_boxes" msg.layout.dim[0].size = len(boxes)/n msg.layout.dim[0].stride = len(boxes) msg.layout.dim[1].label = "nums" msg.layout.dim[1].size = n msg.layout.dim[1].stride = n msg.layout.data_offset = 0 msg.data = boxes box_pub.publish(msg) return
def socket_talker(): ##創建Server node
pub = rospy.Publisher('chatter', Int32MultiArray, queue_size=10)
rospy.init_node(NAME)
rate = rospy.Rate(10) # 10hz
print("Ready to connect")
while not rospy.is_shutdown():
# hello_str = "hello world %s" % rospy.get_time()
state = Int32MultiArray()
state.data = [state_feedback.ArmState, state_feedback.SentFlag]
# rospy.loginfo(state)
pub.publish(state)
rate.sleep()
rospy.spin
def main():
cam = CameraSubscriber()
centroid_module = utils.centroid_module()
best_params_path = "model/model_v2.pickle"
model = utils.create_model(best_params_path)
labels_path = "model/label_colors.json"
labels_dict = utils.labels_colors_dicts(labels_path)
#Sleep until the subscribers are ready.
time.sleep(0.40)
# frame = cam.right_frame
# cv2.imshow("d", frame)
# cv2.waitKey(0)
count = 0
try:
while not rospy.core.is_shutdown():
# rospy.rostime.wallsleep(0.25)
frame = cam.right_frame
final_frame, mask = utils.test_img(frame,model, labels_dict)
print("prediction {:}".format(count))
count += 1
cv2.imshow("img", final_frame)
k = cv2.waitKey(1) & 0xFF
centroids = centroid_module.calculate_centroids(mask)
# print(centroids)
centroids = centroids.reshape(-1).tolist()
arr_to_send = Int32MultiArray()
arr_to_send.data = centroids
cam.multi_arr_pub.publish(arr_to_send)
time.sleep(2)
if k == 27:
cv2.imwrite('./masks/mask1.jpg', mask)
break
# break
# cv2.waitKey(1)
except KeyboardInterrupt:
cv2.imwrite('./masks/mask1.jpg', mask)
print("Shutting down")
cv2.destroyAllWindows()
def main():
rospy.init_node('motion_controller', anonymous=True)
rospy.Subscriber("/dxl_data/present_position_array", Int32MultiArray,
dynamixel_position_callback)
rospy.Subscriber("/dxl_data/present_velocity_array", Int32MultiArray,
dynamixel_velocity_callback)
rospy.Subscriber("/Angle", Float64, robot_position_callback)
pub0 = rospy.Publisher('/program_state', Int32, queue_size=1)
pub1 = rospy.Publisher('/dxl_target/Target_position',
Int32MultiArray,
queue_size=1)
pub2 = rospy.Publisher('/dxl_target/Target_current', Int32, queue_size=1)
sleep_rate = 100 # 100Hz
rate = rospy.Rate(sleep_rate)
dynamixel_goal = np.array([35, 0])
publish_target_position_array = Int32MultiArray(data=dynamixel_goal)
previous_robot_position = 0.
state = input()
print("RUNNING")
pub0.publish(state)
while not rospy.is_shutdown():
robot_velocity = (previous_robot_position -
present_robot_position) * sleep_rate
#print(robot_velocity)
desired_position = alpha * np.arctan(robot_velocity)
publish_target_current = int((control_signal_definition(
present_robot_position, -1 * present_dynamixel_position[1],
desired_position, robot_velocity,
-1 * present_dynamixel_velocity[1]) / torque_constant) * 10 /
(current_scaling_factor))
pub1.publish(publish_target_position_array)
pub2.publish(publish_target_current)
previous_robot_position = present_robot_position
rate.sleep()
def __init__(self, model_name):
self.model_name = model_name
self.wheel_vels = Int32MultiArray()
self.wheel_vels.data = [0, 0]
self.camera_img = None
self.input_size = 8
self.output_size = 2
# move net out
self.net = Net(self.input_size, self.output_size)
self.inputs = torch.ones([1, 8], dtype=torch.float64)
self.inputs = self.inputs.float()
t = threading.Thread(target=self.start)
t.daemon = True
t.start()
def __init__(self):
self.wheel_left = Encoder()
self.wheel_right = Encoder()
self.array_to_publish = Int32MultiArray(data=[0, 0])
GPIO.setmode(GPIO.BOARD)
GPIO.setup(MOTORENCODERRIGHT, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.add_event_detect(MOTORENCODERRIGHT,
GPIO.RISING,
callback=self.wheel_right.tick,
bouncetime=2)
GPIO.setup(MOTORENCODERLEFT, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.add_event_detect(MOTORENCODERLEFT,
GPIO.RISING,
callback=self.wheel_left.tick,
bouncetime=2)
def publish_array():
global explore, arrpub, borders
points = []
for i in range(curr_point.x - range_, curr_point.x + range_):
for j in range(curr_point.y - range_, curr_point.y + range_):
if (((i - curr_point.x) * (i - curr_point.x) + (j - curr_point.y) *
(j - curr_point.y) <= range_ * range_)
and (i <= grid_size - 1 and i >= 0 and j <= grid_sizey - 1
and j >= 0)):
points.append(i)
points.append(j)
else:
continue
a = Int32MultiArray()
a.data = points
arrpub.publish(a)
def netsend(msg, flag=-1, need_unpack=True):
global pro_pub, gesture_id
if msg:
if flag != -1 and flag != -19 and flag != -18:
rospy.loginfo("flag is {}. msg is {}".format(flag, msg))
if need_unpack:
send = []
for i in range(len(msg)):
send.append(int(msg[i][0]))
send.append(int(msg[i][1]))
a = deepcopy(send)
a.append(flag)
else:
a = deepcopy(msg)
a.append(flag)
pro_pub.publish(Int32MultiArray(data=a))
def timer_cb(msg1,msg2,msg3,msg4):
# planes_indices_l = msg1.indices
# planes_indices_r = msg2.indices
# global length_r,length_l,length_cover_r,length_cover_l
length_l=len(msg1.indices)
length_r=len(msg2.indices)
length_cover_l=len(msg3.indices)
length_cover_r=len(msg4.indices)
flag_list=Int32MultiArray()
if length_l < threshould_l:
flag_l= 0
else:
flag_l= 1
if length_r < threshould_r:
flag_r= 0
else:
flag_r= 1
if length_cover_l < threshould_cover_l:
flag_cover_l= 0
else:
flag_cover_l= 1
if length_cover_r < threshould_cover_r:
flag_cover_r= 0
else:
flag_cover_r= 1
# print('in_hand points [r , l] = {}'.format([length_r,length_l]))
print('inhand R', length_r)
print('inhand L', length_l)
print('covered R', length_cover_r)
print('covered L', length_cover_l)
print('holding flag [r , l] = {}'.format([flag_r,flag_l]))
# print('flag_l', flag_l)
# print('flag_r', flag_r)
# print('in_vision points [r , l] = {}'.format([in_vision_r,in_vision_l]))
print('arm coverd flag [r , l] = {}'.format([flag_cover_r,flag_cover_l]))
print('in_vision flag [r , l] = {}'.format([in_vision_r,in_vision_l]))
pub_num_l.publish(length_l)
pub_num_r.publish(length_r)
# pub_drop_flag_l.publish(flag_l)
# pub_drop_flag_r.publish(flag_r)
flag_list.data=[flag_r,flag_l,flag_cover_r,flag_cover_l,in_vision_r,in_vision_l]
pub_drop_flag.publish(flag_list)
def check_leader():
# check which robot is the leader
desired_leader = 0
max = 0
for i in range(len(inrange_count)):
if (inrange_count[i] > max):
max = inrange_count[i]
desired_leader = i
# Notify the user with the highest count
rospy.loginfo('Highest count is at Robot {}. Sees {} robots'.format(
desired_leader + 1, inrange_count[desired_leader]))
rospy.loginfo('All range counts are: {}'.format(inrange_count))
rangePub.publish(Int32MultiArray(data=inrange_count))
leaderPub.publish(desired_leader)
rospy.loginfo('inrange_count is now published')
def motor_angle_publisher():
global counter_array
pub = rospy.Publisher('motor_encoder', Int32MultiArray, queue_size=10)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(1000)
counter_array = Int32MultiArray()
counter_array.data = []
clk1, clk2, dt1, dt2, counter1, counter2, clkLastState1, clkLastState2 = init_motor_encoders(
)
while not rospy.is_shutdown():
clkState1 = GPIO.input(clk1)
clkState2 = GPIO.input(clk2)
dtState1 = GPIO.input(dt1)
dtState2 = GPIO.input(dt2)
if clkState1 != clkLastState1:
if dtState1 != clkState1:
counter1 += 1
else:
counter1 -= 1
print counter1
if clkState2 != clkLastState2:
if dtState2 != clkState2:
counter2 += 1
else:
counter2 -= 1
print counter2
clkLastState1 = clkState1
clkLastState2 = clkState2
counter_array.data = [counter1, counter2]
pub.publish(counter_array)
sleep(0.01)
#rate.sleep()
GPIO.cleanup()
return
def listener():
#global the_map
the_map = Int32MultiArray()
rospy.init_node('Tracking')
rospy.Subscriber('robot1', Int32MultiArray, callback1)
rospy.Subscriber('robot2', Int32MultiArray, callback2)
rospy.Subscriber('robot3', Int32MultiArray, callback3)
rospy.Subscriber('robot4', Int32MultiArray, callback4)
rospy.Subscriber('obst1', Int32MultiArray, callback5)
rospy.Subscriber('obst2', Int32MultiArray, callback6)
rospy.Subscriber('rob1_CurrentPose', Int32MultiArray, callback7)
rospy.Subscriber('rob2_CurrentPose', Int32MultiArray, callback8)
rospy.Subscriber('rob3_CurrentPose', Int32MultiArray, callback9)
rospy.Subscriber('rob4_CurrentPose', Int32MultiArray, callback10)
while not rospy.is_shutdown():
rospy.sleep(1)
def path_planning_routine(): map_valid = create_map() if map_valid: route = pathPlanner(the_map,n,m,dirs,dx,dy,robot_position[0], robot_position[1],goal_position[0],goal_position[1]) update_map_w_route(route) path_squares = get_path_squares(the_map) print "Path is: \n", path_squares, "\n" print "Final Map is: \n", the_map, "\n" publisher_path = np.array(path_squares).flatten().tolist() if ( len(publisher_path) <= 0): publisher_path = robot_position pathPublisher.publish(Int32MultiArray(data=publisher_path)) return path_squares else: print "Map is invalid. No Planning will occur" return 0
def listener():
global pub
pub_arr_key = Int32MultiArray()
pub = rospy.Publisher('x', String, queue_size=10)
rospy.init_node('converter', anonymous=True)
rospy.Subscriber("cmd_vel", Twist, callback)
rate = rospy.Rate(10)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
rate.sleep()
def joyPub(self):
vl, vr = 200.0, 200.0 # define teleop velocity
v, bytes = [0.0] * 2, [
0
] * 2 # allocate space for arrays that will be published
velocity = Float32MultiArray() # objects for ros variables
ros_bytes = Int32MultiArray()
if self.name == 'Xbox Wireless Controller':
joy = pygame.joystick.Joystick(self.joy_id)
joy.init() # initialize object created above
for i in range(joy.get_numaxes()):
axis = joy.get_axis(i)
print('buttons active at %d are %s\n' % (i, axis))
if joy.get_button(
self.B) == 1: # If B is pressed, start sending commands
hat = joy.get_hat(self.hat) # check hat controls for input
if hat[0] == -1: # store desired velocity per input
v = [-vl, vr]
elif hat[0] == 1:
v = [vl, -vr]
elif hat[1] == -1:
v = [-vl, -vr]
elif hat[1] == 1:
v = [vl, vr]
velocity.data = v
self.velPub.publish(velocity)
if joy.get_button(
self.rBumper
) == 1: # turn on vacuum and all brushes if right trigger is active
bytes = [
138, 7
] # motor byte determined from bits available given in create specs
ros_bytes.data = bytes
self.bytePub.publish(
ros_bytes) # publish bytes for writing to create
if joy.get_button(
self.lBumper
) == 1: # turn off vacuum if left trigger is activated
bytes = [138, 0]
ros_bytes.data = bytes
self.bytePub.publish(ros_bytes)
def joyCallback(self, joy_msg):
com_data = [0, 0]
button_msg = Int32()
steering_cmd = joy_msg.axes[self.get_parameter("LEFT_JOY_LR").value]
com_data[0] = roundUp(int(100 * (steering_cmd)), 10)
lt_val = joy_msg.axes[self.get_parameter("LT").value]
rt_val = joy_msg.axes[self.get_parameter("RT").value]
com_data[1] = roundUp(int(100 * (lt_val - rt_val)), 10)
msg = Int32MultiArray(data=com_data)
self.command_pub.publish(msg)
breaking_cmd = joy_msg.buttons[self.get_parameter("B").value]
button_msg.data = breaking_cmd
#self.get_logger().info('Publishing: "%s"' % msg.data)
self.breaking_pub.publish(button_msg)
def socket_talker(): ##創建Server node
pub = rospy.Publisher('chatter', Int32MultiArray, queue_size=10)
rospy.init_node(NAME)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
# hello_str = "hello world %s" % rospy.get_time()
state = Int32MultiArray()
state.data = [1 2]
# rospy.loginfo(state)
pub.publish(state)
rate.sleep()
# a = rospy.Service('arm_mode',arm_mode, Arm_Mode) ##server arm mode data
# s = rospy.Service('arm_pos',arm_data, point_data) ##server arm point data
# b = rospy.Service('speed_mode',speed_mode, Speed_Mode) ##server speed mode data
#c = rospy.Service('grip_mode',grip_mode, Grip_Mode) ##server grip mode data
print ("Ready to connect")
def numpy_to_multiarray(A):
strides = np.cumprod(
A.shape[::-1])[::-1] # unused, but conforms to the ROS spec
layout = MultiArrayLayout(dim=[
MultiArrayDimension(size=d, stride=s)
for d, s in zip(A.shape, strides)
])
if A.dtype == np.float32:
return Float32MultiArray(layout, A.reshape(-1).tolist())
elif A.dtype == np.float64:
return Float64MultiArray(layout, A.reshape(-1).tolist())
elif A.dtype == np.int32:
return Int32MultiArray(layout, A.reshape(-1).tolist())
elif A.dtype == np.int64:
return Int64MultiArray(layout, A.reshape(-1).tolist())
else:
raise TypeError
def camera_depth_callback(msg):
global processing
if (processing == False):
processing = True
time_start = time.clock()
result = Int32MultiArray()
result.data = []
cloud = tr3_nav.transformer.transform(msg)
for row in range(cloud.shape[0]):
for col in range(cloud.shape[1]):
n = 0
x = cloud[row][col][0]
y = cloud[row][col][1]
z = cloud[row][col][2]
if math.isnan(x):
n = 1
x = 0
else:
x = int(x * 1000000)
if math.isnan(y):
n = 1
y = 0
else:
y = int(y * 1000000)
if math.isnan(z):
n = 1
z = 0
else:
z = int(z * 1000000)
result.data.append(n)
result.data.append(x)
result.data.append(y)
result.data.append(z)
pub.publish(result)
time_elapsed = (time.clock() - time_start)
processing = False
###############
# Example 3
# Listen for the writeable modbus registers in any node
def callback(msg):
rospy.loginfo("Modbus register have been written: %s",str(msg.data))
rospy.sleep(2)
sub = rospy.Subscriber("modbus_server/read_from_registers",HoldingRegister,callback,queue_size=500)
###############
###############
# Example 4
# Publisher to write first 20 modbus registers from any node
pub = rospy.Publisher("modbus_server/write_to_registers",HoldingRegister,queue_size=500)
rospy.sleep(1)
msg = HoldingRegister()
msg.data = range(20)
msg2 = HoldingRegister()
msg2.data = range(20,0,-1)
while not rospy.is_shutdown():
pub.publish(msg)
rospy.sleep(1)
pub.publish(msg2)
rospy.sleep(1)
################
rospy.spin()
mws.stopServer()
#################
# Example 2
# Create a listener that show us a message if anything on the readable modbus registers change
rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c")
def showUpdatedRegisters(msg):
rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data))
sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500)
#################
#################
# Example 3
# writing to modbus registers using a standard ros publisher
pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500)
output = HoldingRegister()
output.data = xrange(20,40)
output2 = HoldingRegister()
output2.data = xrange(40,20,-1)
rospy.loginfo("Sending arrays to the modbus server")
while not rospy.is_shutdown():
rospy.sleep(1)
pub.publish(output)
rospy.sleep(1)
pub.publish(output2)
#################
# Stops the listener on the modbus
modclient.stopListening()
#################
# Example 2
# Create a listener that show us a message if anything on the readable modbus registers change
rospy.loginfo("All done. Listening to inputs... Terminate by Ctrl+c")
def showUpdatedRegisters(msg):
rospy.loginfo("Modbus server registers have been updated: %s",str(msg.data))
sub = rospy.Subscriber("modbus_wrapper/input",HoldingRegister,showUpdatedRegisters,queue_size=500)
#################
#################
# Example 3
# writing to modbus registers using a standard ros publisher
pub = rospy.Publisher("modbus_wrapper/output",HoldingRegister,queue_size=500)
output = HoldingRegister()
output.data = [1 for x in xrange(0,6)]
output2 = HoldingRegister()
output2.data = [0 for x in xrange(0,6)]
rospy.loginfo("Sending arrays to the modbus server")
for i in xrange(5):
rospy.sleep(1)
pub.publish(output)
rospy.sleep(1)
pub.publish(output2)
#################
rospy.loginfo("Outputs tests all done, just listening to inputs. Stop listening by pressing Ctrl+c")
rospy.spin()
# Stops the listener on the modbus
