def move_to_vol(self):
self.target = self.coord_deque.popleft()
while not rospy.is_shutdown():
rospy.loginfo_throttle(2, "current_target : " + str(self.target))
# Use the try :... except :
try:
target_service = rospy.ServiceProxy(self.service_name,
TargetCoordinate)
response = target_service(x=self.target[0],
y=self.target[1],
client_name="SCRIPT_NODE_NAME")
# we can also use Header() but you'd have to input the
# frame_id so I figured "client_name" was more direct.
if (response.arrived):
rospy.loginfo_throttle(2, "Yay rover arrived !")
self.target = self.coord_deque.popleft()
if (self.coord_deque.count() == 0):
break
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
rospy.wait_for_service(self.service_name)
rate.sleep()
def magneticsCallback(self, data):
if (data.id != self.balljoint.config['id']):
return
for select,i in zip(self.sensor_select,range(len(data.x))):
angle = self.balljoint.config['sensor_angle'][select][2]
sensor_quat = Quaternion(axis=[0, 0, 1], degrees=-angle)
val = np.array((data.x[select], data.y[select], data.z[select]))
sv = sensor_quat.rotate(val)
# if select >= 14: # the sensor values on the opposite pcb side need to inverted
# quat2 = Quaternion(axis=[1, 0, 0], degrees=200)
# sv = quat2.rotate(sv)
self.b_target[i] = sv
# print(self.b_target)
res = least_squares(self.minimizeFunc, self.pos_estimate_prev, bounds=((-360, -360, -360), (360, 360, 360)),
ftol=1e-15, gtol=1e-15, xtol=1e-15, verbose=0, diff_step=0.01) # ,max_nfev=20
b_field_error = res.cost
rospy.loginfo_throttle(1, "result %.3f %.3f %.3f b-field error %.3f" % (
res.x[0], res.x[1], res.x[2], res.cost))
msg = sensor_msgs.msg.JointState()
msg.header = std_msgs.msg.Header()
msg.header.stamp = rospy.Time.now()
msg.name = [self.body_part + '_axis0', self.body_part + '_axis1', self.body_part + '_axis2']
msg.velocity = [0, 0, 0]
msg.effort = [0, 0, 0]
euler = [res.x[0] / 180 * pi, res.x[1] / 180 * pi, res.x[2] / 180 * pi]
msg.position = [euler[0], euler[1], euler[2]]
self.joint_state.publish(msg)
# if b_field_error < 20000:
self.pos_estimate_prev = res.x
def run(self):
msg_count = 0
try:
bag = rosbag.Bag(
self.bagfile, mode='a' if self.append else 'w')
rate = rospy.Rate(self.lookup_frequency)
last_stamp = rospy.Time()
while not rospy.is_shutdown():
try:
transform = self.tf_buffer.lookup_transform(
self.parent_frame, self.child_frame, rospy.Time())
rate.sleep()
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
if last_stamp == transform.header.stamp:
continue
pose = transformstamped_to_posestamped(transform)
bag.write(self.output_topic, pose, t=pose.header.stamp)
msg_count += 1
last_stamp = transform.header.stamp
rospy.loginfo_throttle(
10, "Recorded {} PoseStamped messages.".format(msg_count))
except rospy.ROSInterruptException:
pass
finally:
bag.close()
rospy.loginfo("Finished recording.")
def run(self):
"""
Run common loop for slave following master. If it loses track stop moving.
"""
#check time since last detection, if lost, kill velocity commands, send lost message over \comm
if rospy.get_time() - self.last_detection > 0.25:
self.lost = True
rospy.loginfo_throttle(5.0, 'Stopping Slave')
self.vel_pub.publish(Twist())
comm = {
'guidance': self.guidance,
'from_slave': True,
'slave_id': self.id,
'target_id': self.target_id,
'lost': self.lost
}
str_cmd = String()
str_cmd.data = json.dumps(comm)
self.comm_pub.publish(str_cmd)
#perform cleaning task at SOI
if self.clean:
rospy.loginfo(
f"Slave {self.id} started cleaning at SOI {self.target_id}")
self.clean_start = rospy.get_time()
self.clean_end = rospy.get_time()
while abs(self.clean_start - self.clean_end) < self.clean_time:
self.perform_clean()
self.clean_end = rospy.get_time()
self.clean = False
rospy.loginfo(
f"Slave {self.id} FINISHED cleaning at SOI {self.target_id}")
self.rate.sleep()
def cmdVelCb(self, req):
x = req.linear.x
th = req.angular.z
## send cmd to gpio here
if x > 0:
GPIO.output(BACKWARD_OUT, False)
GPIO.output(FORWARD_OUT, True)
elif x < 0:
GPIO.output(FORWARD_OUT, False)
GPIO.output(BACKWARD_OUT, True)
else:
GPIO.output(FORWARD_OUT, False)
GPIO.output(BACKWARD_OUT, False)
if th > 0:
GPIO.output(RIGHT_OUT, False)
GPIO.output(LEFT_OUT, True)
elif th < 0:
GPIO.output(LEFT_OUT, False)
GPIO.output(RIGHT_OUT, True)
else:
GPIO.output(LEFT_OUT, False)
GPIO.output(RIGHT_OUT, False)
logstr = "get x, z:" + str(x) + ", " + str(th)
rospy.loginfo_throttle(1, logstr)
def safety_monitor(self):
x = self.ros_data["local_position"].pose.position.x
y = self.ros_data["local_position"].pose.position.y
z = self.ros_data["local_position"].pose.position.z
if z >= 0.35:
self.taken_off = True
#note that z axis have a hystersis, for the shaking avoidance..
state = self.ros_data.get("state", None)
if state is not None:
if state.armed == False or state.mode != "OFFBOARD":
rospy.loginfo_throttle(
1,
"Current status unable to takeoff!\n mode:%s, armed:%s" %
(self.ros_data["state"].mode,
self.ros_data["state"].armed))
rospy.logwarn("user interupted! trying to end this episode...")
return True
else:
rospy.logwarn("abnormal state!") # this should not happen....
return True
# if self.taken_off and (x<-2.5 or x>2.5 or y<-1.5 or y>1.5 or z<0.25 or z>3):
if self.taken_off and (z < 0.25 or z > 5):
rospy.logwarn("land due to safety constraint! pos:%s" % str(
(x, y, z)))
return True
return False
def run(self):
msg_count = 0
try:
bag = rosbag.Bag(self.bagfile, mode='a' if self.append else 'w')
rate = rospy.Rate(self.lookup_frequency)
last_stamp = rospy.Time()
while not rospy.is_shutdown():
try:
transform = self.tf_buffer.lookup_transform(
self.parent_frame, self.child_frame, rospy.Time())
rate.sleep()
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
if last_stamp == transform.header.stamp:
continue
pose = transformstamped_to_posestamped(transform)
bag.write(self.output_topic, pose, t=pose.header.stamp)
msg_count += 1
last_stamp = transform.header.stamp
rospy.loginfo_throttle(
10, "Recorded {} PoseStamped messages.".format(msg_count))
except rospy.ROSInterruptException:
pass
finally:
bag.close()
rospy.loginfo("Finished recording.")
def bbox_tracking(self, ts, bbox, est_pos, frame_gray):
#if self.debug_tracker is None:
#HERE we need a real matching to avoid multiply match to single object
_match_id = self.match_pos(est_pos)
_id = self.estimate_bbox_id(bbox)
if _id is None:
_id = random.randint(100, 1000)
rospy.loginfo_throttle(1.0, "Found new object {}".format(_id))
else:
self.remove_tracker(_id)
#Start new track; fix the bounding box
if _id > self.MAX_DRONE_ID and _match_id is not None:
_id = _match_id
rospy.loginfo_throttle(1.0, "Object {} match to Drone {}".format(_id, _match_id))
bbox.id = _id
if (not self.tracker_only_on_matched) or (self.tracker_only_on_matched and _id < self.MAX_DRONE_ID):
self.start_tracker_tracking(_id, frame_gray, bbox)
self.add_bbox(ts, bbox)
return _id
def get_smocap_camera_from_ros(camera_name):
''' retrieve camera info (extrinsic and intrinsic) from a running smocap '''
camera = smocap.Camera(camera_name)
rospy.init_node('SmocapConfigGetter')
# retrieve camera calibration
cam_info_msg = rospy.wait_for_message(camera_name + '/camera_info',
sensor_msgs.msg.CameraInfo)
camera.set_calibration(
np.array(cam_info_msg.K).reshape(3, 3), np.array(cam_info_msg.D),
cam_info_msg.width, cam_info_msg.height)
rospy.loginfo(' retrieved camera calibration for {}'.format(camera_name))
# retrieve camera localization
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
while not camera.is_localized():
try:
world_to_camo_transf = tf_buffer.lookup_transform(
source_frame='world',
target_frame='{}_optical_frame'.format(camera_name),
time=rospy.Time(0))
world_to_camo_t, world_to_camo_q = utils.t_q_of_transf_msg(
world_to_camo_transf.transform)
camera.set_location(world_to_camo_t, world_to_camo_q)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.loginfo_throttle(1., " waiting to get camera location")
rospy.loginfo(' retrieved camera location')
return camera
def main():
rospy.init_node('SVEA_sim_purepursuit')
# initialize simulated model and control interface
simple_bicycle_model = SimpleBicycleState(*init_state, dt=dt)
ctrl_interface = ControlInterface(vehicle_name).start()
rospy.sleep(0.5)
# start background simulation thread
simulator = SimSVEA(vehicle_name, simple_bicycle_model, dt)
simulator.start()
rospy.sleep(0.5)
# log data
x = []
y = []
yaw = []
v = []
t = []
# pure pursuit variables
lastIndex = len(cx) - 1
target_ind = pure_pursuit.calc_target_index(simple_bicycle_model, cx, cy)
# simualtion + animation loop
time = 0.0
while lastIndex > target_ind and not rospy.is_shutdown():
# compute control input via pure pursuit
steering, target_ind = \
pure_pursuit.pure_pursuit_control(simple_bicycle_model, cx, cy, target_ind)
ctrl_interface.send_control(steering, target_speed)
# log data
x.append(simple_bicycle_model.x)
y.append(simple_bicycle_model.y)
yaw.append(simple_bicycle_model.yaw)
v.append(simple_bicycle_model.v)
t.append(time)
# update for animation
if show_animation:
to_plot = (simple_bicycle_model, x, y, steering, target_ind)
animate_pure_pursuit(*to_plot)
else:
rospy.loginfo_throttle(1.5, simple_bicycle_model)
time += dt
if show_animation:
plt.close()
to_plot = (simple_bicycle_model, x, y, steering, target_ind)
animate_pure_pursuit(*to_plot)
plt.show()
else:
# just show resulting plot if not animating
to_plot = (simple_bicycle_model, x, y)
plot_trajectory(*to_plot)
plt.show()
def callback(config, level): # callback called when a parameter is updated
rospy.loginfo_throttle(
1,
'''Reconfigure Request: {pick}, {dock}, {undock}, {place}, {home}, {return}, {return_pose_trans_x}, \
{return_pose_trans_y}, {return_pose_rot_x}, {return_pose_rot_y}, {return_pose_rot_z}, {return_pose_rot_w} '''
.format(**config))
return config
def timer_callback(self, event):
# Get current camera coords
stamp = rospy.Time.now()
src_frame = '/world'
dst_frame = '/head_mount_kinect_rgb_optical_frame'
try:
self.listener.waitForTransform(src_frame,
dst_frame,
stamp,
timeout=rospy.Duration(1))
except Exception as e:
rospy.logerr(e)
return
dst_pose = self.listener.lookupTransform(src_frame, dst_frame, stamp)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = src_frame
pose_stamped.header.stamp = stamp
pose_stamped.pose.position.x = dst_pose[0][0]
pose_stamped.pose.position.y = dst_pose[0][1]
pose_stamped.pose.position.z = dst_pose[0][2]
pose_stamped.pose.orientation.x = dst_pose[1][0]
pose_stamped.pose.orientation.y = dst_pose[1][1]
pose_stamped.pose.orientation.z = dst_pose[1][2]
pose_stamped.pose.orientation.w = dst_pose[1][3]
# Check the change
delta_saving_pos, delta_saving_ori = np.inf, np.inf
vel_pos, vel_ori = 0, 0
if self.last_pose_stamped is not None:
delta_pos, delta_ori = compute_pose_delta(
pose_stamped.pose, self.last_pose_stamped.pose)
delta_time = stamp - self.last_pose_stamped.header.stamp
vel_pos = delta_pos / delta_time.to_sec()
vel_ori = delta_ori / delta_time.to_sec()
if self.last_saved_pose_stamped is not None:
delta_saving_pos, delta_saving_ori = compute_pose_delta(
pose_stamped.pose, self.last_saved_pose_stamped.pose)
self.last_pose_stamped = pose_stamped
logging_msg = ('delta_saving_pos: {}, delta_saving_ori: {}, '
'vel_pos: {}, vel_ori: {}'.format(
delta_saving_pos, delta_saving_ori, vel_pos,
vel_ori))
# Save according to the change
if (delta_saving_pos > self.delta_pos
and delta_saving_ori > self.delta_ori
and vel_pos < self.velocify_pos
and vel_ori < self.velocify_ori):
rospy.loginfo(logging_msg)
rospy.loginfo(colored('Sending saving result', 'blue'))
res = self.trigger()
rospy.loginfo(
colored(
'Saving request result success: {}'.format(res.success),
'green' if res.success else 'red'))
if res.success:
self.last_saved_pose_stamped = pose_stamped
else:
rospy.loginfo_throttle(1, logging_msg)
def connect(self):
with self.__lock:
rospy.loginfo_throttle(
1,
'Modbus connecting to %s:%d' % (self.__address, self.__port))
while not rospy.is_shutdown():
try:
self.__modbus_socket = socket.socket(
socket.AF_INET, socket.SOCK_STREAM)
self.__modbus_socket.settimeout(self.__socket_timeout)
self.__modbus_socket.connect((self.__address, self.__port))
rospy.loginfo('Successfully connected to modbus')
break
except socket.timeout:
rospy.logwarn('Timeout on modbus connect. Reconnecting.')
except socket.gaierror as ex:
rospy.logwarn(
'''\
getaddrinfo() failed for server %s. Reconnecting.\
''', self.__address)
except socket.error as ex:
if ex.errno in {
errno.ECONNABORTED, errno.ECONNREFUSED,
errno.EADDRINUSE, errno.EHOSTUNREACH
}:
rospy.logwarn(
'Modbus connect failed (%s). Reconnecting.', ex)
else:
raise
time.sleep(self.__retry_time)
def update_active_subscriber(self):
'''sets which subscriber shall be the chosen one ("active") based on error and status'''
# first: do they actually work right now
enabled = [sub.is_enabled() for sub in self.subscribers]
# build list of minimum error of enabled subscribers from index to end
min_err = [np.inf] * (len(self.subscribers) + 1)
for i in range(len(min_err) - 2, -1, -1):
min_err[i] = min(self.subscriber_errors[i],
min_err[i + 1]) if enabled[i] else min_err[i + 1]
# determine active subscriber
for i in range(len(self.subscribers)):
if enabled[i]:
if (self.subscriber_errors[i] < self.threshold # error is ok
# or i == len(self.subscribers) - 1 # last subscriber has the smallest error
or self.subscriber_errors[i] <
min_err[i + 1] # following errors are even worse
):
if self.subscriber_errors[i] >= self.threshold:
rospy.loginfo_throttle(
1,
'ImuController: choosing %s despite err > threshold (lowest fallback error: %.4f)'
% (type(
self.subscribers[i]).__name__, min_err[i + 1]))
if not self.is_active_subscriber(i):
self.switch_subscriber(i)
return
# all subscribers are inactive
self.active_subscriber_idx = None
def updateBattery(self):
'''
Method to simulate the battery discharge
'''
elapsed_time = 1.0/self.real_freq
# 3600 secs -> power consumption
# elapsed_time -> x
battery_consumption = (self._current_power_consumption * elapsed_time) / 3600
self._current_battery_amperes -= battery_consumption
if self._current_battery_amperes < 0.0:
self._current_battery_amperes = 0.0
elif self._current_battery_amperes > self._battery_amperes:
self._current_battery_amperes = self._battery_amperes
current_battery_percentage = 100*(self._current_battery_amperes / self._battery_amperes)
current_battery_voltage_index = 0
for i in range(len(self._battery_voltage)-1):
if current_battery_percentage < self._battery_voltage[i][1] and current_battery_percentage >= self._battery_voltage[i+1][1]:
current_battery_voltage_index = i+1
self._current_battery_voltage = self._battery_voltage[current_battery_voltage_index][0]
rospy.loginfo_throttle(30, '%s::updateBattery: battery_amperes = %lf, percentage = %lf, voltage = %.3lf' % (
self.node_name, self._current_battery_amperes, current_battery_percentage, self._current_battery_voltage))
return
def publish(self):
if self.rpm_msg.thruster_1_rpm != 0 or not self.published_zero_rpm_once:
self.thruster_pub.publish(self.rpm_msg)
zero = self.rpm_msg.thruster_1_rpm == 0
if zero:
rospy.loginfo(">>>>> Published 0 RPM")
else:
rospy.loginfo_throttle(1, "Rpm:{}".format(self.rpm_msg.thruster_1_rpm))
self.published_zero_rpm_once = zero
if self.vec_msg.thruster_horizontal_radians != 0 or \
self.vec_msg.thruster_vertical_radians != 0 or \
not self.published_zero_vec_once:
self.vector_pub.publish(self.vec_msg)
zero = self.vec_msg.thruster_horizontal_radians == 0 and\
self.vec_msg.thruster_vertical_radians == 0
if zero:
rospy.loginfo(">>>>> Published 0 thrust vector")
else:
rospy.loginfo_throttle(1, "Vec:{},{}".format(self.vec_msg.thruster_horizontal_radians, self.vec_msg.thruster_vertical_radians))
self.published_zero_vec_once = zero
# reset the stuff
self.rpm_msg.thruster_1_rpm = 0
self.rpm_msg.thruster_2_rpm = 0
self.vec_msg.thruster_horizontal_radians = 0
self.vec_msg.thruster_vertical_radians = 0
def callback(msg):
flag = 0
if msg.battery < config.MONITOR['low_battery_voltage']:
rospy.logwarn_throttle(config.MONITOR['log_period'],
"Battery voltage low: %f" % (msg.battery))
flag = 1
if msg.current_left > config.MONITOR['current_limit_each']:
rospy.logwarn_throttle(
config.MONITOR['log_period'],
"Left motor current high: %f" % (msg.current_left))
flag = 1
if msg.current_right > config.MONITOR['current_limit_each']:
rospy.logwarn_throttle(
config.MONITOR['log_period'],
"Right motor current high: %f" % (msg.current_right))
flag = 1
if msg.pcb_temperature > config.MONITOR['pcb_temperature_limit']:
rospy.logwarn_throttle(
config.MONITOR['log_period'],
"PCB temperature high: %f" % (msg.pcb_temperature))
flag = 1
if msg.rc == config.MONITOR['rc_no_signal']:
rospy.logwarn_throttle(config.MONITOR['log_period'],
"No radio controller signal")
flag = 1
if flag == 0:
rospy.loginfo_throttle(config.MONITOR['log_period'],
"No abnormalities detected")
def on_vel(self, msg):
"""
:param Twist msg:
:return: None
"""
q = self.last_pose.pose.orientation
_, _, yaw = tf.transformations.euler_from_quaternion(
[q.x, q.y, q.z, q.w])
# TODO: check the signs on these math equations
self.vel.linear.z = msg.linear.z
self.vel.linear.x = msg.linear.x * np.cos(yaw) + msg.linear.y * np.sin(
yaw)
self.vel.linear.y = msg.linear.y * np.cos(yaw) - msg.linear.x * np.sin(
yaw)
rospy.loginfo_throttle(
0.5, "Vel correction: \tth=({}) \tin={} \tout={}".format(
yaw, (msg.linear.x, msg.linear.y),
(self.vel.linear.x, self.vel.linear.y)))
self.vel.angular = msg.angular
try:
self.vel_pub.publish(self.vel)
except:
rospy.logerr("Unable to publish velocity")
def data_cb(self, src, img, stamp):
# TODO : save data in cam_ and run data_cb in step()
# instead of inside the callback. May run into strange race conditions.
now = rospy.Time.now()
if (now - stamp).to_sec() > 0.5:
rospy.loginfo_throttle(
10.0, 'incoming data too old: [{}] @ {}'.format(src, stamp))
# too old
return
if not (src in self.cam_):
# should usually not reach here - error
return
cam = self.cam_[src]
if not cam.has_info_:
# should usually not reach here, maybe the first few frames
return
if self.dbg_:
# continuous detection
rsp = self.detect_cb(
DetectRequest(source=src,
track=False,
cid=DetectRequest.CID_NULL))
if rsp.success:
box = BoxUtils.convert(t.box_, BoxUtils.FMT_NCCWH,
BoxUtils.FMT_NYXYX)
draw_bbox(img, box, cls=None)
cv2.imshow('win', img)
cv2.waitKey(1)
def takeOff(self):
goal = Goal()
goal.p.x = self.pose.position.x
goal.p.y = self.pose.position.y
goal.p.z = self.pose.position.z
goal.psi = quat2yaw(self.pose.orientation)
goal.power = True
#Turn on the motors
alt_taken_off = self.pose.position.z + 1.0
#Altitude when hovering after taking off
#Note that self.pose.position is being updated in the parallel callback
######## Commented for simulations
while (abs(self.pose.position.z - alt_taken_off) > 0.2):
goal.p.z = min(goal.p.z + 0.0035, alt_taken_off)
rospy.sleep(0.004)
rospy.loginfo_throttle(
0.5, "Taking off..., error={}".format(self.pose.position.z -
alt_taken_off))
self.sendGoal(goal)
########
rospy.sleep(0.1)
self.whoplans.value = self.whoplans.PANTHER
self.sendWhoPlans()
def detect_by_yolo(self, img_gray):
ts = rospy.get_time()
loader = transforms.Compose([
transforms.ToTensor(),
])
img_size = self.img_size
img_ = cv2.resize(img_gray, (img_size, img_size))
image = loader(img_).cuda()
image = image.view(1, img_size, img_size).expand(3, -1, -1)
with torch.no_grad():
if not self.use_tensorrt:
detections = self.model(image.unsqueeze(0))
#print("DTS ", detections.shape)
detections = non_max_suppression(detections, self.conf_thres, self.nms_thres)
else:
#print("Using converted")
x, x8 = self.model_backbone(image.unsqueeze(0))
#print("Finish backbone", x.shape, x8.shape, x13.shape)
detections = self.model_end(x, x8)
#print("Finish end", detections.shape)
#print(detections)
detections = non_max_suppression(detections, self.conf_thres, self.nms_thres)
rospy.loginfo_throttle(1.0, "Detection use time {:f}ms".format((rospy.get_time() - ts)*1000))
if detections[0] is not None:
return detections[0].cpu().numpy()
return []
def main():
rospy.init_node('nmea_topic_serial_reader')
nmea_pub = rospy.Publisher("nmea_sentence", Sentence, queue_size=1)
serial_port = rospy.get_param('~port', '/dev/ttyUSB0')
serial_baud = rospy.get_param('~baud', 4800)
# Get the frame_id
frame_id = RosNMEADriver.get_frame_id()
try:
GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2)
while not rospy.is_shutdown():
data = GPS.readline().strip()
sentence = Sentence()
sentence.header.stamp = rospy.get_rostime()
sentence.header.frame_id = frame_id
sentence.sentence = data
# log the raw nmea (hope we will see some GGA)
rospy.loginfo_throttle(1, sentence)
nmea_pub.publish(sentence)
except rospy.ROSInterruptException:
GPS.close() # Close GPS serial port
def execute(self, userdata):
# This method is called periodically while the state is active.
# Main purpose is to check state conditions and trigger a corresponding outcome.
# If no outcome is returned, the state will stay active.
#Logger.logerr('[ServiceFollowTrajectory]: execute')
if not self._connected:
Logger.logerr('[ServiceFollowTrajectory]: not connected to %s' % self._control_manager_diagnostics_topic)
return 'failed'
diff_time = rospy.get_time() - self._start_time
if self._sub_cont_diag.has_msg(self._control_manager_diagnostics_topic):
rospy.loginfo_throttle(5,'[ServiceFollowTrajectory]: has diagnostics msg at time %s' % (str(rospy.get_time())))
message = self._sub_cont_diag.get_last_msg(self._control_manager_diagnostics_topic)
self._sub_cont_diag.remove_last_msg(self._control_manager_diagnostics_topic)
if diff_time >= 1.0 and not message.tracker_status.tracking_trajectory:
Logger.loginfo('[ServiceFollowTrajectory]: Successfully ended following of trajectory %s' % self._control_manager_diagnostics_topic)
return 'successed'
else:
return
if self._failed or diff_time >= len(userdata.scanning_trajectory) * 0.2 * 1.2:
Logger.logerr('Failed follow trajectory \'{}\' within time {} s out of {} s'.format(self._service_topic_follow, str(diff_time),len(userdata.scanning_trajectory) * 0.2 * 1.2))
return 'failed'
def receive_data(self):
msg = self.bus.recv(0.01) # Timeout in seconds. None: Wait until data is received.
if msg is None:
rospy.logerr('Timeout occurred, no message.')
elif self.verbose:
rospy.loginfo_throttle(0.5,msg)
return msg
def _publish_pose_stamped_all_drones(self, swarm_log):
# type: (List[DroneLog]) -> None
for drone_log in swarm_log:
pos = drone_log.position
namespace = drone_log.namespace
self._pub_pose_stamped(namespace, *pos)
rospy.loginfo_throttle(5, "published PoseStamped for drones")
def ros_callback(self, data):
x_test = np.array([
data.x[0], data.y[0], data.z[0], data.x[1], data.y[1], data.z[1],
data.x[2], data.y[2], data.z[2]
])
x_test = x_test.reshape((1, 9))
try:
(trans, rot) = listener.lookupTransform('/world', '/tracker_1',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
with self.graph.as_default(
): # we need this otherwise the precition does not work ros callback
quat = self.model.predict(x_test)
# pos = self.model.predict(x_test)
rospy.loginfo_throttle(
1, (quat[0, 0], quat[0, 1], quat[0, 2], quat[0, 3]))
norm = numpy.linalg.norm(quat)
q = (quat[0, 0] / norm, quat[0, 1] / norm, quat[0, 2] / norm,
quat[0, 3] / norm)
# print "predicted: ",(pos[0,0],pos[0,1],pos[0,2])
self.br.sendTransform(trans, (q[0], q[1], q[2], q[3]),
rospy.Time.now(), "shoulderOrientation",
"world")
def joint_angles_callback(self, msg):
"""
Reads the latest position of the robot and sets an
appropriate command to move the robot to the target
"""
# read the current joint angles from the robot
curr_pos = np.array([
msg.joint1, msg.joint2, msg.joint3, msg.joint4, msg.joint5,
msg.joint6, msg.joint7
]).reshape((7, 1))
rospy.loginfo_throttle(5, "current joint angles: {}".format(curr_pos))
# convert to radians
curr_pos = curr_pos * (np.pi / 180.0)
self.last_dof = curr_pos
# update target position to move to depending on:
# - if moving to START of desired trajectory or
# - if moving ALONG desired trajectory
self.update_target_pos(curr_pos)
# update cmd from PID based on current position
self.cmd = self.update(curr_pos)
for i in range(7):
if self.cmd[i][i] > self.max_cmd[i][i]:
self.cmd[i][i] = self.max_cmd[i][i]
if self.cmd[i][i] < -self.max_cmd[i][i]:
self.cmd[i][i] = -self.max_cmd[i][i]
def cc_ctrl(self):
rospy.loginfo_throttle(1, "Running CC control")
if (self.state.vx > 0.0):
# get lhpt
lhdist = 7 # todo determine from velocity
s_lh = self.state.s + lhdist
d_lh = self.cc_dref
Xlh, Ylh = ptsFrenetToCartesian(np.array([s_lh]), \
np.array([d_lh]), \
np.array(self.pathlocal.X), \
np.array(self.pathlocal.Y), \
np.array(self.pathlocal.psi_c), \
np.array(self.pathlocal.s))
rho_pp = self.pp_curvature(self.state.X, self.state.Y,
self.state.psi, Xlh[0], Ylh[0])
delta_out = rho_pp * (self.lf + self.lr) # kinematic feed fwd
else:
Xlh = 0.0
Ylh = 0.0
delta_out = 0.0
self.vx_error = self.cc_vxref - self.state.vx
if (self.robot_name == "gotthard"):
k = 500
elif (self.robot_name == "rhino"):
k = 1500
else:
k = 0
print "ERROR: incorrect /robot_name"
dc_out = k * self.vx_error
return delta_out, dc_out, Xlh, Ylh
def tag_callback(self, msg):
"""
Updates the pose for detected apriltags
Args:
msg (AprilTagDetectionArray): detected april tags in current view
"""
for tag in msg.detections:
tag_id = tag.id
rospy.loginfo_throttle(5.0, "Detected tag: %d" % tag_id)
#Follow tag if its the leader and in guidance mode
if tag_id == self.master_id and self.guidance:
self.lost = False
self.last_detection = rospy.get_time()
rospy.loginfo_throttle(5.0, "Following Master")
self.tag_follow(tag_id)
#Goto target tag
elif tag_id == self.target_id:
dist = self.tag_follow(tag_id)
if dist > 3.0:
return
rospy.loginfo("Found SOI, Task complete.")
self.last_detection = rospy.get_time()
self.guidance = False
if dist < 1.0:
rospy.loginfo("Setting clean to true")
self.clean = True
self.lost = False
def _control_to_pose(self, goal_pose, position_gain, rotation_gain, abs_vx, abs_vy, abs_vyaw):
if self._goal_reached(*self._get_target_delta_in_robot_frame(goal_pose)):
rospy.loginfo("We are already there")
return
rospy.loginfo("Starting alignment ....")
while not rospy.is_shutdown():
dx, dy, dyaw = self._get_target_delta_in_robot_frame(goal_pose)
if self._goal_reached(dx, dy, dyaw):
break
rospy.loginfo_throttle(1.0, "Aligning .. Delta = {} {} {}".format(dx, dy, dyaw))
self._cmd_vel_publisher.publish(Twist(
linear=Vector3(
x=_clamp(abs_vx, position_gain * dx),
y=_clamp(abs_vy, position_gain * dy)
),
angular=Vector3(z=_clamp(abs_vyaw, rotation_gain * dyaw))
))
self._rate.sleep()
rospy.loginfo("Goal reached")
def behave(self):
# Set head mode
self.head_look_for_ball()
# Stop the robot
self.stopAtBegin()
# Wait for game to start
while self.allow_to_move == False and not rospy.is_shutdown():
time.sleep(0.1)
rospy.loginfo_throttle(5, "Waiting for game to start")
# Start core behavior
while not rospy.is_shutdown():
# Walk while steering torwards the ball
self.goToBall()
# Chose what to do if we reached the ball (pre determind)
if self.goal_behavior:
# Center the goal and kick
self.moonWalk()
if self.kick_behavior:
# Kick forwards
self.kick()
else:
# Run against the ball
self.runThroughBall()
# wait til the next iteration starts
time.sleep(1)
def main():
rospy.init_node('depth_to_worldstate', anonymous=False)
global image_pub
image_pub = rospy.Publisher("image_topic", Image, queue_size=5)
world_pub = rospy.Publisher("world_state_status", worldstate, queue_size=5)
rospy.Subscriber('/camera/color/image_raw', Image, image_callback)
world_state_msg = worldstate()
rospy.loginfo("spinning now...")
rospy.spin()
while red_point is None or blue_point is None or yellow_point is None:
rospy.loginfo_throttle(1, "Haven't seen all items yet...")
output_str = ""
output_str += "- {name: agent , x: " + str(5) + " , y: " + str(
5) + "}\n"
output_str += "- {name: cube1 , x: " + str(
red_point[0]) + " , y: " + str(red_point[1]) + "}\n"
output_str += "- {name: cube2 , x: " + str(
yellow_point[0]) + " , y: " + str(yellow_point[1]) + "}\n"
output_str += "- {name: crafting_table , x: " + str(
blue_point[0]) + " , y: " + str(blue_point[1]) + "}"
with open("./data/data.yaml", "w") as outputfile:
outputfile.write(output_str)
def timer_callback(self, event):
# Get current camera coords
stamp = rospy.Time.now()
src_frame = '/world'
dst_frame = '/head_mount_kinect_rgb_optical_frame'
try:
self.listener.waitForTransform(src_frame, dst_frame, stamp,
timeout=rospy.Duration(1))
except Exception as e:
rospy.logerr(e)
return
dst_pose = self.listener.lookupTransform(src_frame, dst_frame, stamp)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = src_frame
pose_stamped.header.stamp = stamp
pose_stamped.pose.position.x = dst_pose[0][0]
pose_stamped.pose.position.y = dst_pose[0][1]
pose_stamped.pose.position.z = dst_pose[0][2]
pose_stamped.pose.orientation.x = dst_pose[1][0]
pose_stamped.pose.orientation.y = dst_pose[1][1]
pose_stamped.pose.orientation.z = dst_pose[1][2]
pose_stamped.pose.orientation.w = dst_pose[1][3]
# Check the change
delta_saving_pos, delta_saving_ori = np.inf, np.inf
vel_pos, vel_ori = 0, 0
if self.last_pose_stamped is not None:
delta_pos, delta_ori = compute_pose_delta(
pose_stamped.pose, self.last_pose_stamped.pose)
delta_time = stamp - self.last_pose_stamped.header.stamp
vel_pos = delta_pos / delta_time.to_sec()
vel_ori = delta_ori / delta_time.to_sec()
if self.last_saved_pose_stamped is not None:
delta_saving_pos, delta_saving_ori = compute_pose_delta(
pose_stamped.pose, self.last_saved_pose_stamped.pose)
self.last_pose_stamped = pose_stamped
logging_msg = ('delta_saving_pos: {}, delta_saving_ori: {}, '
'vel_pos: {}, vel_ori: {}'
.format(delta_saving_pos,
delta_saving_ori, vel_pos, vel_ori))
# Save according to the change
if (delta_saving_pos > self.delta_pos and
delta_saving_ori > self.delta_ori and
vel_pos < self.velocify_pos and
vel_ori < self.velocify_ori):
rospy.loginfo(logging_msg)
rospy.loginfo(colored('Sending saving result', 'blue'))
res = self.trigger()
rospy.loginfo(colored('Saving request result success: {}'
.format(res.success),
'green' if res.success else 'red'))
if res.success:
self.last_saved_pose_stamped = pose_stamped
else:
rospy.loginfo_throttle(1, logging_msg)
def test_log(self):
# we can't do anything fancy here like capture stdout as rospy
# grabs the streams before we do. Instead, just make sure the
# routines don't crash.
real_stdout = sys.stdout
real_stderr = sys.stderr
try:
try:
from cStringIO import StringIO
except ImportError:
from io import StringIO
sys.stdout = StringIO()
sys.stderr = StringIO()
import rospy
rospy.loginfo("test 1")
self.assert_("test 1" in sys.stdout.getvalue())
rospy.logwarn("test 2")
self.assert_("[WARN]" in sys.stderr.getvalue())
self.assert_("test 2" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logerr("test 3")
self.assert_("[ERROR]" in sys.stderr.getvalue())
self.assert_("test 3" in sys.stderr.getvalue())
sys.stderr = StringIO()
rospy.logfatal("test 4")
self.assert_("[FATAL]" in sys.stderr.getvalue())
self.assert_("test 4" in sys.stderr.getvalue())
# logXXX_throttle
for i in range(3):
sys.stdout = StringIO()
rospy.loginfo_throttle(3, "test 1")
if i == 0:
self.assert_("test 1" in sys.stdout.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stdout.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 1" in sys.stdout.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logwarn_throttle(3, "test 2")
if i == 0:
self.assert_("test 2" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 2" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logerr_throttle(3, "test 3")
if i == 0:
self.assert_("test 3" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 3" in sys.stderr.getvalue())
for i in range(3):
sys.stderr = StringIO()
rospy.logfatal_throttle(3, "test 4")
if i == 0:
self.assert_("test 4" in sys.stderr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1:
self.assert_("" == sys.stderr.getvalue())
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 4" in sys.stderr.getvalue())
finally:
sys.stdout = real_stdout
sys.stderr = real_stderr
