def measure(self, initv, interval, repeat):
da_all = []
speed = 5000
msg = Int64()
msg.data = speed
self.pub_speed.publish(speed)
for i in range(repeat + 1):
da = []
vol = initv + interval * i
msg = Float64()
msg.data = vol
self.pub_vol.publish(vol)
time.sleep(3)
ret = reader.piv_reader()
da.append(ret[0])
da.append(ret[1])
da.append(ret[2])
print(da)
da_all.append(da)
np.savetxt("yfactor_{0}.txt".format(self.ut),
np.array(da_all),
delimiter=" ")
speed = 0
msg = Int64()
msg.data = speed
self.pub_speed.publish(speed)
yf.pv_iv_plot()
def talker(name="talker", anon=False):
rospy.init_node(name, anonymous=anon)
time.sleep(0.5)
print("Started node %s" % rospy.get_name())
chatter_topic = "/chatter"
counter_topic = "/counter"
print("Publishing to %s" % chatter_topic)
pub1 = rospy.Publisher(chatter_topic, String, queue_size=10)
time.sleep(0.5)
print("Publishing to %s" % counter_topic)
pub2 = rospy.Publisher(counter_topic, Int64, queue_size=10)
cnt = 0
rate = rospy.Rate(1.) # 1hz
while not rospy.is_shutdown():
chatter_msg = "hello world %d" % (cnt)
counter_msg = Int64(data=cnt)
if cnt % 10 == 1:
rospy.loginfo("%d. %s <- %s" % (cnt, chatter_topic, chatter_msg))
pub1.publish(chatter_msg)
rate.sleep()
if cnt % 10 == 1:
rospy.loginfo("%d. %s <- %s" % (cnt, counter_topic, counter_msg))
pub2.publish(Int64(data=cnt))
rate.sleep()
cnt = cnt + 1
print("Finished")
def step(self):
if self.playing:
# don't use ros time because we don't want to rely on simulation time
sleep(self.period)
else:
while not self.should_step and not self.playing and not self.done:
sleep(0.01)
if self.fwd:
if self.idx < self.max_idx - 1:
self.idx += 1
else:
if self.auto_play:
self.done = True
elif self.loop:
self.idx = 0
else:
self.playing = False
else:
if self.idx > 0:
self.idx -= 1
t_msg = Int64()
t_msg.data = self.time_steps[self.idx]
self.time_pub.publish(t_msg)
self.should_step = False
max_t_msg = Int64()
max_t_msg.data = self.time_steps[-1]
self.max_time_pub.publish(max_t_msg)
return self.done
def initializeDefaultParam(self):
"""Initialise the grasp planner with default values
"""
camera_pose = Pose()
camera_pose.orientation.w = 1
num_samples = Int64(400)
num_threads = Int64(1)
tabin_radius = Float64(0.03)
hand_radius = Float64(0.08)
self._graspPlanner.configureAgilePlannerLocalizer(
camera_pose, num_samples, num_threads, tabin_radius, hand_radius)
finger_width = Float64(0.01)
hand_outer_diameter = Float64(0.09)
hand_depth = Float64(0.06)
init_bite = Float64(0.01)
hand_height = Float64(0.02)
self._graspPlanner.configureAgilePlannerHand(finger_width,
hand_outer_diameter,
hand_depth, init_bite,
hand_height)
pass
def MotionCallback(self, data):
v1 = data.linear.x
theta1 = data.angular.z
max_speed = self.get_parameter(
"max_speed").get_parameter_value().double_value
turn_rate = self.get_parameter(
"turn_rate").get_parameter_value().double_value
self.get_logger().info("turnRate: " + str(turn_rate) +
" max_speed: " + str(max_speed))
# Calculating speed of each of the motors
leftSpeed = max_speed * v1 - turn_rate * theta1
rightSpeed = max_speed * v1 + turn_rate * theta1
self.get_logger().info("leftSpeed: " + str(leftSpeed) +
" rightSpeed: " + str(rightSpeed))
#Sending messages to the motor nodes.
lmsg = Int64()
lmsg.data = int(clip(leftSpeed, -98, 98))
self.leftWheelPub.publish(lmsg)
rmsg = Int64()
rmsg.data = int(clip(rightSpeed, -98, 98))
self.rightWheelPub.publish(rmsg)
self.get_logger().info("Updated Driving Motors- Right Speed:" +
str(rightSpeed) + " Left Speed:" +
str(leftSpeed))
def camera_callback(self, data):
rospy.loginfo("April tag detector loginfo")
# We select bgr8 because its the OpneCV encoding by default
cv_image = self.bridge_object.imgmsg_to_cv2(data,
desired_encoding="bgr8")
frame = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# April tag detector
tag_detected = False
detector = apriltag.Detector()
result = detector.detect(frame)
tagID = 0
global mission_stage_msg
if len(result) == 1:
tag_detected = True
tag = result[0].tag_family
tagID = result[0].tag_id
cx = result[0].center[0]
cy = result[0].center[1]
corner_points = result[0].corners
if len(result) > 1:
print("Multiple tags detected")
pub2 = rospy.Publisher("/current_tag", Int64, queue_size=1)
msg2 = Int64()
msg2.data = 0
if tag_detected:
print("Tag detected")
print('tag ID = ', tagID)
#print(result)
# publish current detected tag if any
msg2.data = tagID
if tag_detected == False:
print('No tag detected')
print('\n')
# publish mission stage
if mission_stage_msg == 0 and tagID == 8:
mission_stage_msg = 1
if mission_stage_msg == 1 and tagID == 9:
mission_stage_msg = 2
if mission_stage_msg == 2 and tagID == 6:
mission_stage_msg = 3
if mission_stage_msg == 3 and tagID == 4:
mission_stage_msg = 4
pub = rospy.Publisher("/mission_stage", Int64, queue_size=1)
msg = Int64()
msg.data = mission_stage_msg
pub.publish(msg)
pub2.publish(msg2)
def pubMsg(vr, vl):
rospy.loginfo('right: %d', vr)
rospy.loginfo('left: %d', vl)
msg_vr = Int64()
msg_vr.data = vr
listener_R(msg_vr)
msg_vl = Int64()
msg_vl.data = vl
listener_L(msg_vl)
def callback_raw_image(self, msg):
""" Callback: new image came is -> Process it
After detecting the ball, three topics are published:
camera1/image_processed
camera1/ball_visible
camera1/ball_center_radius
"""
self.image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
greenLower = (50, 50, 20)
greenUpper = (70, 255, 255)
blurred = cv2.GaussianBlur(self.image, (11, 11), 0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, greenLower, greenUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
#cv2.imshow('mask', mask)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
center = [0, 0]
radius = 0
visible = False
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 10:
# draw the circle and centroid on the frame,
# then update the list of tracked points
visible = True
cv2.circle(self.image, (int(x), int(y)), int(radius),
(0, 255, 255), 2)
cv2.circle(self.image, center, 5, (0, 0, 255), -1)
self.pub_img.publish((self.bridge.cv2_to_imgmsg(self.image, 'bgr8')))
self.pub_vis.publish(visible)
cr = BallCenterRadius()
cr.visible = Bool(visible)
cr.center_x = Int64(center[0])
cr.center_y = Int64(center[1])
cr.radius = Int64(int(radius))
self.pub_cr.publish(cr)
def __init__(self):
self.command_left = Int64()
self.command_right = Int64()
self.received_twist = None
rospy.init_node('Twist2int64')
# rospy.Subscriber('motor/twist/cmd_vel', Twist, self.callback)
rospy.Subscriber('cmd_vel', Twist, self.callback)
self.pub_right = rospy.Publisher(
'right_motor/cmd_vel', Int64, queue_size=10)
self.pub_left = rospy.Publisher(
'left_motor/cmd_vel', Int64, queue_size=10)
def plan(self, object):
"""Compute a grasp candidate.
"""
pcd_name = "../../data/models/pointclouds/" + object.name + ".pcd"
self._graspPlanner.computeAgileGraspFromPointCloud(
String(pcd_name), String(self._svm_name), Int64(self._min_inliers))
axis_vec = self._graspPlanner.getAgileGraspCandidateAxisAt(Int64(0))
approach_vec = self._getAgileGraspCandidateApproachAt(Int64(0))
#other???? this is what I can provide: the center of the grasp (grasp position between the end of the finger tips),
#the width of the object contained in the grasp, the center of the grasp projected onto the back of the hand.
return (approach_vec, axis_vec)
def test_ros_message_with_int64(self):
from std_msgs.msg import Int64
expected_dictionary = { 'data': -0x7FFFFFFFFFFFFFFF }
message = Int64(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def publish(self):
# pin_msg is of type Int64
pins_msg = Int64()
# pack pins to pin_msg
pins_msg.data = self.pins
# pub
self.pub.publish(pins_msg)
def low_pri_work(self):
msg = Int64()
msg.data = 29481
while True:
self.publisher_2.publish(msg)
time.sleep(0.2)
def face_detection_cb(msg):
pub = rospy.Publisher('motor1/command', Int64, queue_size=1)
face_msg = FaceArrayStamped()
motor_command_msg = Int64()
face_pos = [0, 0]
global motor_angle
# face check
if len(msg.faces):
face = msg.faces[0].face
face_pos[0] = face.x
face_pos[1] = face.y
# check face position. left or right
if face_pos[0] <= image_center[0]:
motor_angle -= 1
else:
motor_angle += 1
motor_command_msg.data = motor_angle
# print
print "face_pos(x, y): ({} {})".format(face_pos[0], face_pos[1])
print "/motor1/command: {}\n".format(motor_command_msg.data)
# publish
pub.publish(motor_command_msg)
else:
print "no faces"
def callback_number(msg):
global counter
counter += msg.data
new_msg = Int64()
new_msg.data = counter
pub.publish(new_msg)
rospy.loginfo("I Publish the counter value: %s", counter)
def main():
pwm_msg = Int64()
pwm_msg.data = 30
pubPWM.publish(pwm_msg)
subGoal = rospy.Subscriber('goal_rig', Point, get_goal)
sub_pose = rospy.Subscriber('zed/zed_node/pose_twist', Twist, get_pose)
sub_xboxTakeover = rospy.Subscriber('xbox_takeover', Int64, xboxTakeover)
sub_pose_for_test= rospy.Subscriber('zed/zed_node/pose', PoseStamped, save_pose)
sub_path_for_test= rospy.Subscriber('Astar_path', Path, save_path)
reachedGoal = Int64()
reachedGoal.data = 1
pubReachedGoal.publish(reachedGoal)
while not rospy.is_shutdown():
doStuff()
rate.sleep()
def __init__(self):
self.bridge = CvBridge()
self.image_received = False
self.detector = apriltag.Detector()
self.apriltag_detection_status = Bool()
self.apriltagID = Int64()
rospy.logwarn("AprilTag Detection PC Node [ONLINE]...")
# rospy shutdown
rospy.on_shutdown(self.cbShutdown)
# Subscribe to CompressedImage msg
self.image_topic = "/pc_cv_camera/image_raw"
self.image_sub = rospy.Subscriber(self.image_topic, Image,
self.cbImage)
# Publish to Bool msg
self.apriltagStatus_topic = "/pc_apriltag_detection_status"
self.apriltagStatus_pub = rospy.Publisher(self.apriltagStatus_topic,
Bool,
queue_size=10)
# Publish to Int64 msg
self.apriltagID_topic = "/pc_apriltag_detection_ID"
self.apriltagID_pub = rospy.Publisher(self.apriltagID_topic,
Int64,
queue_size=10)
# Allow up to one second to connection
rospy.sleep(1)
def __init__(self):
# Properties
self.filename = OUT_DATA_FILENAME
self._data = []
self._data_size = 0
# Subscriptions
self._encoder_msg = Int64()
self._encoder_sub = rospy.Subscriber('/encoder', Int64,
self.encoder_callback)
self._cmd_vel_msg = Twist()
self._cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist,
self.cmd_vel_callback)
# Initialise servo driver
self._servo_driver = curio_base.lx16a_driver.LX16ADriver()
self._servo_driver.set_port(SERVO_SERIAL_PORT)
self._servo_driver.set_baudrate(SERVO_BAUDRATE)
self._servo_driver.set_timeout(SERVO_TIMEOUT)
self._servo_driver.open()
rospy.loginfo('Open connection to servo bus board')
rospy.loginfo('is_open: {}'.format(self._servo_driver.is_open()))
rospy.loginfo('port: {}'.format(self._servo_driver.get_port()))
rospy.loginfo('baudrate: {}'.format(self._servo_driver.get_baudrate()))
rospy.loginfo('timeout: {}'.format(self._servo_driver.get_timeout()))
def publish(self):
# action_msg is of type Int64
action_msg = Int64()
# pack action
action_msg.data = self.action
# pub
self.pub1.publish(action_msg)
def callback_number(msg):
global counter
counter += msg.data
new_msg = Int64()
new_msg.data = counter
pub.publish(new_msg)
rospy.loginfo(counter)
def __init__(self):
# Publisher
self._encoder_msg = Int64()
self._encoder_pub = rospy.Publisher('/encoder', Int64, queue_size=10)
# Subscriber
self._cmd_vel_msg = Twist()
self._cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist,
self.cmd_vel_callback)
# Initialise encoder filter
self._encoder_filter = LX16AEncoderFilter(MODEL_FILENAME, WINDOW)
# Initialise servo driver
self._servo_driver = LX16ADriver()
self._servo_driver.set_port(SERVO_SERIAL_PORT)
self._servo_driver.set_baudrate(SERVO_BAUDRATE)
self._servo_driver.set_timeout(SERVO_TIMEOUT)
self._servo_driver.open()
rospy.loginfo('Open connection to servo bus board')
rospy.loginfo('is_open: {}'.format(self._servo_driver.is_open()))
rospy.loginfo('port: {}'.format(self._servo_driver.get_port()))
rospy.loginfo('baudrate: {}'.format(self._servo_driver.get_baudrate()))
rospy.loginfo('timeout: {}'.format(self._servo_driver.get_timeout()))
def test_dictionary_with_int64(self):
from std_msgs.msg import Int64
expected_message = Int64(data = -0x7FFFFFFFFFFFFFFF)
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/Int64', dictionary)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def publish_pendulum(self):
#action_msg is of type Int64
pendulum_msg = Int64()
#pack action
pendulum_msg.data = self.pendulumAction
# pub
self.pubPendulum.publish(pendulum_msg)
def static_data_cb(data):
global A, pub, tmp
ipdb.set_trace()
A = sum(data.taxels[0].values) + sum(data.taxels[1].values)
msg = Int64()
msg.data = A - tmp
pub.publish(msg)
def callback_number_counter(msg):
global counter
counter += msg.data
new_msg = Int64()
new_msg.data = counter
pub.publish(new_msg)
def decideOnAction():
global closeEnough
global angleDev
global xbox
if (xbox == 0):
pwm_msg = Int64()
pwm_msg.data = 30
pubPWM.publish(pwm_msg)
action = 4
distance, angle = calculateAngleAndDistance()
if (xbox == 0) :
if ((angle > 3 or angle < -3) and distance < reverseDist and distance >= closeEnough): ## if not reached goal and point not within acceptable angles, try backing
action = 1
elif angle <= angleDev and angle >= -angleDev and distance >= closeEnough: ##if kinda infront and at least 'closeEnough' meters to the point
action = 7
elif angle > angleDev and distance >= closeEnough: ##if to the left and at least selected distance to the point
action = 6
elif angle < -angleDev and distance >= closeEnough: ##if to the right and at least selected distance to the point
action = 8
else:
action = 4 #stand still if no conditions are fulfilled
return action
def do_joint_cmd(self, data=None):
if data is not None:
cmd = Int64()
cmd.data = data
self.JointCmdPub.publish(cmd)
else:
self.JointCmdPub.publish(self.JointCmd)
pass
def callback_function(self, msg):
"""
This function is a callback function which sums the numbers.
"""
self.counter += msg.data
new_msg = Int64()
new_msg.data = self.counter
self.pub.publish(new_msg)
def publish_timer_cb(self):
# Pulls data and publishes them over the autogenerated ROS2 topic
self.reading_data_lock = True
all_device_chrc_data = self.ble_adapter.pull_device_data()
self.reading_data_lock = False
for addr, chrc_data in all_device_chrc_data.items():
for chrc_handle, chrc_value in chrc_data.items():
chrc_type = self.yaml_config['characteristics'][chrc_handle][
'msg_type']
# Generate a unique topic_id for this device/chrc combo
topic_id = self._generate_topic_id(addr, chrc_handle)
# Create new publisher and subscriber if new device is found
if topic_id not in self.pubs.keys():
try:
self._generate_publisher(addr, chrc_handle, chrc_type)
except ValueError:
Utils.logger.error(
'An error occurred generating publishers.')
return
if topic_id not in self.subs.keys():
try:
self._generate_subscriber(addr, chrc_handle, chrc_type)
except ValueError:
Utils.logger.error(
'An error occurred generating subscribers.')
return
pub = self.pubs[topic_id]
if chrc_type == 'byte':
# Hacky way to display raw bytes in ROS2 topics
msg = String()
msg.data = str(chrc_value)
elif chrc_type == 'string':
msg = String()
msg.data = str(chrc_value)
elif chrc_type == 'int':
msg = Int64()
msg.data = int(chrc_value)
elif chrc_type == 'float':
msg = Float64()
msg.data = float(chrc_value)
elif chrc_type == 'bool':
msg = Bool()
msg.data = bool(chrc_value)
else:
Utils.logger.error(f'{chrc_type} is invalid.')
return
Utils.logger.info(f'Published data: {topic_id}:{msg}')
pub.publish(msg)
def generate_cloud_indexed_msg(self):
np_cloud, idx = self.extract_indices()
msg = CloudIndexed()
header = Header()
# header.frame_id = "xtion_rgb_optical_frame"
header.frame_id = "camera_depth_optical_frame"
header.stamp = rospy.Time.now()
msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(header, np_cloud.tolist())
msg.cloud_sources.view_points.append(Point(0, 0, 0))
for i in xrange(np_cloud.shape[0]):
msg.cloud_sources.camera_source.append(Int64(0))
for i in idx[0]:
msg.indices.append(Int64(i))
return msg
