def motor_speed_send(a,b,c,d,e,f):
global types_array
pub = rospy.Publisher('robot/desired_velocity', TwistStamped, queue_size=10)
r = rospy.Rate(5)
main = TwistStamped()
stepper = Twist()
stepper.linear.x = a
stepper.linear.y = b
stepper.linear.z = c
stepper.angular.x = d
stepper.angular.y = e
stepper.angular.z = f
main.twist = stepper
now = rospy.get_rostime()
main.header.stamp.secs = now.secs
main.header.stamp.nsecs = now.nsecs
main.header.frame_id = "robot/desired_velocity"
pub.publish(main)
rospy.loginfo("Stepper Motor sent %s", main)
r.sleep()
def navigate(self):
rate = self.rospy.Rate(10) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
print "Time since last vel:", (time.time() - self.lastVelTime)
if (self.inCollision or (time.time() - self.lastVelTime)>1.0):
# print "inCollision"
msg.twist.linear.x = 0 #self.x
msg.twist.linear.y = 0 #self.y
msg.twist.linear.z = 0 #self.z
msg.twist.angular.z = 0
else:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
msg.twist.angular.z = self.yaw
print "Sending vel"
#print "collision:", self.inCollision
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 0 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, yaw)
#msg.pose.orientation = Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def pose_callback(data):
# extract x and y position information
x = data.pose.pose.position.x
y = data.pose.pose.position.y
# package and broadcast
p = Point()
p.x = x
p.y = y
# send it
pub_beacon.publish(p)
# and broadbast all odoms on a common channel
pub_odom_all.publish(data)
# publish pose info for viewing
pose_out = PoseStamped()
pose_out.header = data.header
pose_out.header.frame_id = '/world'
pose_out.pose = data.pose.pose
pub_pose.publish(pose_out)
# and twist stamped for DMS interface
vel_out = TwistStamped()
vel_out.header = data.header
vel_out.header.frame_id = '/world'
vel_out.twist = data.twist.twist
pub_vel.publish(vel_out)
# transform
tf_broadcast.sendTransform((x, y, 0.0),
(data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w),
rospy.Time.now(),frame_name,"/world")
def handle_convert(data):
pub = rospy.Publisher('/navigation_vel', TwistStamped)
m = TwistStamped()
m.twist = data
m.header = Header()
m.header.stamp = rospy.Time.now()
pub.publish(m)
def callback(msg):
global pub
output = TwistStamped()
output.header.stamp = rospy.Time.now()
output.header.frame_id = sys.argv[1]
output.twist = msg
pub.publish(output)
def onTwist(self,msg): newMsg = TwistStamped() head = std_msgs.msg.Header() newMsg.twist = msg head.stamp = rospy.Time.now() newMsg.header = head self.twist_pub.publish(newMsg)
def default(self, ci='unused'):
twist = TwistStamped()
twist.header = self.get_ros_header()
# Fill twist-msg with the values from the sensor
twist.twist.linear.x = self.data['velocity'][0]
twist.twist.linear.y = self.data['velocity'][1]
twist.twist.linear.z = self.data['velocity'][2]
self.publish(twist)
def default(self, ci='unused'):
twist = TwistStamped()
twist.header = self.get_ros_header()
twist.twist.linear.x = self.data['world_linear_velocity'][0]
twist.twist.linear.y = self.data['world_linear_velocity'][1]
twist.twist.linear.z = self.data['world_linear_velocity'][2]
twist.twist.angular.x = self.data['angular_velocity'][0]
twist.twist.angular.y = self.data['angular_velocity'][1]
twist.twist.angular.z = self.data['angular_velocity'][2]
self.publish(twist)
def ProcessXlateImage(data):
global PrevDiameter
global p_x
global p_z_ang
global TAG_DIAMETER
global TagFound
global LastTwist
InputTags = data
NewTwist = TwistStamped()
if InputTags.tag_count > 0:
TagFound = True
# +Z_fwd_cam = +Z_base - points up
# +Y_fwd_cam = +Y_base - points left
# +X_fwd_cam = +X_base - points forward
NewTwist.twist.linear.x = InputTags.tags[0].diameter - TAG_DIAMETER
# NewTwist.twist.linear.y = InputTags.tags[0].x - ( FWD_IMAGE_WIDTH/2 ) #( IMAGE_WIDTH/2 ) - InputTags.tags[0].y
# NewTwist.twist.linear.z = InputTags.tags[0].y - ( FWD_IMAGE_HEIGHT/2 ) #( IMAGE_HEIGHT/2 ) - InputTags.tags[0].x
NewTwist.twist.angular.z = InputTags.tags[0].x - (IMAGE_WIDTH / 2) # ( IMAGE_WIDTH/2 ) - InputTags.tags[0].y #
# PrevDiameter = InputTags.tags[0].diameter
# rospy.Publisher("image_pos", NewTwist )
ProcessImagePosition(NewTwist)
# Keep some history for when the tag disappears
while len(PrevVector) >= MAX_HISTORY:
PrevVector.pop(0)
PrevVector.append(NewTwist.twist)
else:
# Extrapolate history
try:
NewTwist.twist = PrevVector.pop(0)
print "Use History %d" % len(PrevVector)
NewTwist.twist = LastTwist
# Save off some history
except IndexError, e:
# Ran out of history
NewTwist.twist.linear.x = 0
NewTwist.twist.linear.y = 0
NewTwist.twist.linear.z = 0
NewTwist.twist.angular.z = 0
if TagFound:
p_x.Reset()
p_z_ang.Reset()
TagFound = False
pub = rospy.Publisher("cmd_vel", Twist)
pub.publish(NewTwist.twist)
def parse_twist_stamped(twist_stamped):
rospy.loginfo('twist_stamped')
rospy.logdebug('parse_twist_stamped: parsing ' + str(twist_stamped))
f_from = YamlPars0r.get_dict_value(twist_stamped, 'from')
f_pose = YamlPars0r.get_dict_value(twist_stamped, 'pose')
twist_msg = YamlPars0r.parse_twist(f_pose)
header_msg = Header()
if f_from is not None:
header_msg.frame_id = f_from
twist_stamped_msg = TwistStamped(
header=header_msg
)
if twist_msg is not None:
twist_stamped_msg.twist = twist_msg
return twist_stamped_msg
def navigate(self):
print("navigate")
rate = self.rospy.Rate(20) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
msg.twist.angular.z = self.yaw
self.pub.publish(msg) # it publish it to the hardware ( the motors)
print "Published velocity to auto x:" + str(self.x) + " y:" + str(self.y) + " z:" + str(self.z) + " yaw:" + str(self.yaw)
rate.sleep()
def twistUpdate(self):
if self.curr_state:
x_des, v_des, R_des, w_des = self.getGoalState(self.curr_state)
e_x, e_v, e_R, e_w = self.getErrors(self.curr_state)
twist_cmd = TwistStamped()
twist_cmd.header = Header()
twist_cmd.header.stamp = rospy.Time.now()
twist_cmd.twist.linear.x = -self.k_x * e_x[0] - self.k_v * e_v[0] + v_des[0]
twist_cmd.twist.linear.y = -self.k_x * e_x[1] - self.k_v * e_v[1] + v_des[1]
twist_cmd.twist.linear.z = -self.k_x * e_x[2] - self.k_v * e_v[2] + v_des[2]
twist_cmd.twist.angular.x = -self.k_R * e_R[0] - self.k_w * e_w[0] + w_des[0]
twist_cmd.twist.angular.y = -self.k_R * e_R[1] - self.k_w * e_w[1] + w_des[1]
twist_cmd.twist.angular.z = -self.k_R * e_R[2] - self.k_w * e_w[2] + w_des[2]
self.twist_pub.publish(twist_cmd)
self.publishGoal(self.curr_state)
return
def twist_cb(self, twist):
# get teleop twist message
twist_stamped = TwistStamped()
twist_stamped.twist = twist
if not (
twist.linear.x == 0
and twist.linear.y == 0
and twist.linear.z == 0
and twist.angular.x == 0
and twist.angular.y == 0
and twist.angular.z == 0
):
# give it a header
header = Header()
header.stamp = rospy.Time.now()
twist_stamped.header = header
# publish the command
self.teleop_stamped_pub.publish(twist_stamped)
def navigate(self):
rate = self.rospy.Rate(10) # 10hz
msg = TwistStamped()
msg.header = Header()
msg.header.frame_id = "base_footprint"
msg.header.stamp = rospy.Time.now()
while 1:
msg.twist.linear.x = self.x
msg.twist.linear.y = self.y
msg.twist.linear.z = self.z
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 0 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, yaw)
#msg.pose.orientation = Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def process_joy_data_callback(data): linear_x = data.axes[1] * max_linear_velocity linear_y = data.axes[0] * max_linear_velocity angular_z = data.axes[3] * max_angular_velocity seq = data.header.seq twist_stamped_msg = TwistStamped() twist_stamped_msg.twist = Twist() twist_stamped_msg.twist.linear = Vector3() twist_stamped_msg.twist.angular = Vector3() twist_stamped_msg.twist.linear.x = linear_x twist_stamped_msg.twist.linear.y = linear_y twist_stamped_msg.twist.linear.z = 0 twist_stamped_msg.twist.angular.x = 0 twist_stamped_msg.twist.angular.y = 0 twist_stamped_msg.twist.angular.z = angular_z twist_stamped_msg.header.seq = seq twist_stamped_msg.header.frame_id = '/base_link' twist_stamped_msg.header.stamp = data.header.stamp pub.publish(twist_stamped_msg)
def timer_cb(self, event):
enable = False
twist = Twist()
fx = self.cur_wrench.force.x
fy = self.cur_wrench.force.y
fz = self.cur_wrench.force.z
if fx > self.dz_x_max:
twist.linear.x = self.k_f * (fx - self.dz_x_max)
enable = True
elif fx < self.dz_x_min:
twist.linear.x = self.k_f * (fx - self.dz_x_min)
enable = True
if fy > self.dz_y_max:
twist.linear.y = self.k_f * (fy - self.dz_y_max)
enable = True
elif fy < self.dz_y_min:
twist.linear.y = self.k_f * (fy - self.dz_y_min)
enable = True
if fz > self.dz_z_max:
twist.linear.z = self.k_f * (fz - self.dz_z_max)
enable = True
elif fz < self.dz_z_min:
twist.linear.z = self.k_f * (fz - self.dz_z_min)
enable = True
if enable:
twist.linear.x = self.saturate_vel(twist.linear.x)
twist.linear.y = self.saturate_vel(twist.linear.y)
twist.linear.z = self.saturate_vel(twist.linear.z)
twist_s = TwistStamped()
twist_s.header.stamp = rospy.get_rostime()
twist_s.twist = twist
self.pub_cmd_vel.publish(twist_s)
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
print(vels(speed, turn))
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
twist = TwistStamped()
twist.header.stamp = rospy.Time.now()
twist.linear.x = x * speed
twist.linear.y = y * speed
twist.linear.z = z * speed
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = th * turn
pub.publish(twist)
except Exception as e:
print(e)
finally:
twist = TwistStamped()
twist.header.stamp = rospy.Time.now()
def __init__(self):
#Initialise
self.clap_detector = ClapDetector()
self.vision = Vision()
# state
self.m_ready = True
self.wagging = False
self.web_cmd = ""
# topic root
self.topic_root = '/' + os.getenv("MIRO_ROBOT_NAME") + '/'
#Configure ROS interface
#Publishers
self.velocity_pub = rospy.Publisher(self.topic_root +
"control/cmd_vel",
TwistStamped,
queue_size=0)
self.cosmetic_joints_pub = rospy.Publisher(self.topic_root +
"control/cosmetic_joints",
Float32MultiArray,
queue_size=0)
self.illum_pub = rospy.Publisher(self.topic_root + "control/illum",
UInt32MultiArray,
queue_size=0)
self.kinematic_joints_pub = rospy.Publisher(self.topic_root +
"control/kinematic_joints",
JointState,
queue_size=0)
self.audio_tone_pub = rospy.Publisher(self.topic_root + "control/tone",
UInt16MultiArray,
queue_size=0)
# self.param_pub = rospy.Publisher(self.topic_root + "control/params", Float32MultiArray, queue_size=0)
self.push_pub = rospy.Publisher(self.topic_root + "core/push",
miro.msg.push,
queue_size=0)
#Create objects to hold published data
self.velocity = TwistStamped()
self.kin_joints = JointState()
self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"]
self.kin_joints.position = [
0.0, miro.constants.LIFT_RAD_CALIB, 0.0, 0.0
]
self.cos_joints = Float32MultiArray()
self.cos_joints.data = [0.0, 0.5, 0.0, 0.0, 0.0, 0.0]
self.tone = UInt16MultiArray()
self.tone.data = [0, 0, 0]
self.illum = UInt32MultiArray()
self.illum.data = [
0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
0xFFFFFFFF
]
# self.params = Float32MultiArray()
# self.params.data = [721.0, 15.0]
#Create Variables to store recieved data
self.sonar_range = None
self.light_array = [None, None, None, None]
self.cliff_array = None
self.head_touch = None
self.body_touch = None
#Arrays to hold image topics
self.cam_left_image = None
self.cam_right_image = None
#Create object to convert ROS images to OpenCV format
self.image_converter = CvBridge()
#Create resource for controlling body_node
self.pars = miro.utils.PlatformPars()
self.cam_model = miro.utils.CameraModel()
self.frame_w = 0
self.frame_h = 0
#Timer variables
self.pause_flag = False
self.timer_end_time = 0.0
self.time_now = 0.0
#Start thread
self.updated = False
self.update_thread = Thread(target=self.update)
self.update_thread.start()
self.thread_running = True
#Subscribe to sensors
self.touch_body_sub = rospy.Subscriber(self.topic_root +
"sensors/touch_body",
UInt16,
self.touch_body_callback,
tcp_nodelay=True)
self.touch_head_sub = rospy.Subscriber(self.topic_root +
"sensors/touch_head",
UInt16,
self.touch_head_callback,
tcp_nodelay=True)
self.mics_sub = rospy.Subscriber(self.topic_root + "sensors/mics",
Int16MultiArray,
self.mics_callback,
tcp_nodelay=True)
#Subscribe to Camera topics
self.cam_left_sub = rospy.Subscriber(self.topic_root +
"sensors/caml/compressed",
CompressedImage,
self.cam_left_callback,
tcp_nodelay=True)
self.cam_right_sub = rospy.Subscriber(self.topic_root +
"sensors/camr/compressed",
CompressedImage,
self.cam_right_callback,
tcp_nodelay=True)
# last subscription is to sensors_package because that drives the clock
self.sensors_sub = rospy.Subscriber(self.topic_root +
"sensors/package",
miro.msg.sensors_package,
self.sensors_callback,
tcp_nodelay=True)
def follow_path(self, path):
'''Moves a manipulator so that it follows the given path. If whole body
motion is enabled and some points on the path lie outside the reachable
workspace, the base is moved accordingly as well. The path is followed
by sending velocity commands.
At each time step, the following rule is used for calculating the
desired velocity command:
v = k * (path_{i+1} - path_{i}) + g * (path_{i} - p)
where:
* v is the velocity (x, y, and z - no angular velocity)
* p is the current position of the end effector
* g is a feedback gain
* k is a feedforward gain
If whole body motion is used and the minimum singular value of the manipulator's
Jacobian falls below a predefined threshold, v is split between the arm and the base as
v_{arm} = v * c
v_{base} = v * (1 - c)
where:
* c = (\sigma_{min} - \sigma_{low}) / (\sigma{high} - \sigma_{low})
* \sigma_{min} is the current minimum singular value of the manipulator's Jacobian
* \sigma_{high} and \sigma_{low} are upper and lower thresholds on the
minimum sigma value (determined experimentally)
Keyword arguments:
path: np.array -- a 2D array of points (each row represents a point)
'''
# sanity check - we only consider the path if it contains any points
if path is None or path.shape[0] == 0:
rospy.logerr(
'[move_arm/dmp/follow_path] No points in path; aborting execution'
)
return
rospy.loginfo('[move_arm/dmp/follow_path] Executing motion')
trans, _ = self.get_transform(self.odom_frame_name,
self.palm_link_name, rospy.Time(0))
current_pos = np.array([trans[0], trans[1], trans[2]])
distance_to_goal = np.linalg.norm((path[-1] - current_pos))
self.motion_completed = False
self.motion_cancelled = False
cmd_count = 0
while not self.motion_completed and \
not self.motion_cancelled and \
not rospy.is_shutdown():
trans, _ = self.get_transform(self.odom_frame_name,
self.palm_link_name, rospy.Time(0))
current_pos = np.array([trans[0], trans[1], trans[2]])
# if the end effector has reached the goal (within the allowed
# tolerance threshold), we stop the motion
distance_to_goal = np.linalg.norm((path[-1] - current_pos))
if distance_to_goal <= self.goal_tolerance:
self.motion_completed = True
break
path_point_distances = [
np.linalg.norm(p - current_pos) for p in path
]
min_dist_idx = np.argmin(path_point_distances)
next_goal_idx = -1
if min_dist_idx == path.shape[0] - 1:
next_goal_idx = min_dist_idx
else:
next_goal_idx = min_dist_idx + 1
vel = self.feedforward_gain * (
path[next_goal_idx] - path[min_dist_idx]
) + self.feedback_gain * (path[next_goal_idx] - current_pos)
# we limit the speed if it is above the allowed limit
velocity_norm = np.linalg.norm(vel)
if velocity_norm > self.linear_vel_limit:
vel = vel * self.linear_vel_limit / velocity_norm
vel_arm = np.array(vel)
vel_base = np.zeros(3)
# if we want to use whole body control and the minimum
# sigma value is below the allowed threshold, we split
# the velocity command between the arm and the base
if self.use_whole_body_control and \
self.min_sigma_value is not None and \
self.min_sigma_value < self.sigma_threshold_upper:
# we set the arm and base velocity based on the value of the
# so-called capability coefficient, which is calculated as
# (\sigma_{min} - \sigma_{low}) / (\sigma{high} - \sigma_{low})
c = (self.min_sigma_value - self.sigma_threshold_lower) / (
self.sigma_threshold_upper - self.sigma_threshold_lower)
vel_arm[0:2] = vel[0:2] * c
vel_base[0:2] = vel[0:2] * (1 - c)
odom_vel_vector = Vector3Stamped()
odom_vel_vector.header.seq = cmd_count
odom_vel_vector.header.frame_id = self.odom_frame_name
odom_vel_vector.vector.x = vel_base[0]
odom_vel_vector.vector.y = vel_base[1]
odom_vel_vector.vector.z = vel_base[2]
base_vel_vector = self.tf_listener.transformVector3(
self.base_link_frame_name, odom_vel_vector)
twist_base = Twist()
twist_base.linear.x = base_vel_vector.vector.x
twist_base.linear.y = base_vel_vector.vector.y
twist_base.linear.z = base_vel_vector.vector.z
self.vel_publisher_base.publish(twist_base)
twist_arm = TwistStamped()
twist_arm.header.seq = cmd_count
twist_arm.header.frame_id = self.odom_frame_name
twist_arm.twist.linear.x = vel_arm[0]
twist_arm.twist.linear.y = vel_arm[1]
twist_arm.twist.linear.z = vel_arm[2]
self.vel_publisher_arm.publish(twist_arm)
cmd_count += 1
# stop arm and base motion after converging
twist_arm = TwistStamped()
twist_arm.header.seq = cmd_count
twist_arm.header.frame_id = self.odom_frame_name
self.vel_publisher_arm.publish(twist_arm)
twist_base = Twist()
if self.use_whole_body_control:
self.vel_publisher_base.publish(twist_base)
def proc_imu(quat1, acc, gyro):
# New info:
# https://github.com/thalmiclabs/myo-bluetooth/blob/master/myohw.h#L292-L295
# Scale values for unpacking IMU data
# define MYOHW_ORIENTATION_SCALE 16384.0f
# See myohw_imu_data_t::orientation
# define MYOHW_ACCELEROMETER_SCALE 2048.0f
# See myohw_imu_data_t::accelerometer
# define MYOHW_GYROSCOPE_SCALE 16.0f
# See myohw_imu_data_t::gyroscope
if not any(quat1):
# If it's all 0's means we got no data, don't do anything
return
h = Header()
h.stamp = rospy.Time.now()
# frame_id is node name without /
h.frame_id = rospy.get_name()[1:]
h2 = Header()
h2.stamp = rospy.Time.now()
h2.frame_id = "imu_global"
# We currently don't know the covariance of the sensors with each other
cov = [0, 0, 0, 0, 0, 0, 0, 0, 0]
quat = Quaternion(quat1[1] / 16384.0, quat1[2] / 16384.0,
quat1[3] / 16384.0, quat1[0] / 16384.0)
# Normalize the quaternion and accelerometer values
quatNorm = sqrt(quat.x * quat.x + quat.y * quat.y + quat.z * quat.z +
quat.w * quat.w)
normQuat = Quaternion(quat.x / quatNorm, quat.y / quatNorm,
quat.z / quatNorm, quat.w / quatNorm)
direction = 1.0
normAcc = Vector3(direction * acc[0] / 2048.0 * 9.80665,
direction * acc[1] / 2048.0 * 9.80665,
direction * acc[2] / 2048.0 * 9.80665)
normGyro = Vector3(gyro[0] / 16.0 * 3.141592 / 180.0,
gyro[1] / 16.0 * 3.141592 / 180.0,
gyro[2] / 16.0 * 3.141592 / 180.0)
imu = Imu(h, normQuat, cov, normGyro, cov, normAcc, cov)
imuPub.publish(imu)
roll, pitch, yaw = euler_from_quaternion(
[normQuat.x, normQuat.y, normQuat.z, normQuat.w])
oriPub.publish(Vector3(roll, pitch, yaw))
oriDegPub.publish(Vector3(degrees(roll), degrees(pitch), degrees(yaw)))
posePub.publish(PoseStamped(h, Pose(Point(0.0, 0.0, 0.0), normQuat)))
null_vector = Vector3(0, 0, 0)
accTwi = Twist(normAcc, null_vector)
acc_twist = TwistStamped(h, accTwi)
accTwistPub.publish(acc_twist)
velTwi = Twist(null_vector, normGyro)
velTwist = TwistStamped(h, velTwi)
velTwistPub.publish(velTwist)
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = h2.frame_id
t.child_frame_id = h.frame_id
t.transform.translation.x = 0
t.transform.translation.y = 0
t.transform.translation.z = 0
t.transform.rotation.x = normQuat.x
t.transform.rotation.y = normQuat.y
t.transform.rotation.z = normQuat.z
t.transform.rotation.w = normQuat.w
br.sendTransform(t)
T_global_raw = posemath.fromMsg(Pose(Point(0.0, 0.0, 0.0), normQuat))
acc_in_raw = PyKDL.Vector(normAcc.x, normAcc.y, normAcc.z)
gyro_in_raw = PyKDL.Vector(normGyro.x, normGyro.y, normGyro.z)
acc_in_global = T_global_raw * acc_in_raw
gyro_in_global = T_global_raw * gyro_in_raw
acc_in_global_msg = Vector3(acc_in_global[0], acc_in_global[1],
acc_in_global[2] - direction * 9.80665)
accTwi = Twist(acc_in_global_msg, null_vector)
acc_twist = TwistStamped(h2, accTwi)
accTwistPubGlobal.publish(acc_twist)
gyro_in_global_msg = Vector3(gyro_in_global[0], gyro_in_global[1],
gyro_in_global[2])
velTwi = Twist(null_vector, gyro_in_global_msg)
velTwist = TwistStamped(h, velTwi)
velTwistPubGlobal.publish(velTwist)
imu = Imu(h, Quaternion(0, 0, 0, 1), cov, gyro_in_global_msg, cov,
acc_in_global_msg, cov)
imuPubGlobal.publish(imu)
# # # # #
# DemoSystem init
# Import core parameters (from client_demo line 221)
pars = pars.CorePars()
# ActionTemplate init
# Initialise private clock object (from action_types line 63)
# clock = ActionClock()
clock = 0
# KC_M (from client_demo line 231)
kc_m = miro.utils.kc_interf.kc_miro()
# Create movement and orientation objects
velocity = TwistStamped() # (from interface line 84)
kin_joints = JointState() # (from interface line 86)
# # # # #
# From action_orient : start
# Called from action_types : descending (209)
# Called from node_action : tick (343)
# Called from client_demo : callback_sensors_package (406)
# get current gaze target in HEAD
gaze_HEAD = miro.utils.kc_interf.kc_view_to_HEAD(
0.0,
CURRENT_ELEV, # Should be current gaze elevation
pars.action.orient_gaze_target_radius)
def create_twist(self, velocity, angular):
tw = TwistStamped()
tw.twist.linear.x = velocity
tw.twist.angular.z = angular
return tw
def one_ico_learning(self,
cur_ang,
reflective,
predictive,
sim: bool = True,
mc_scaling: float = 1.0,
upper_thresh: float = 1.0,
ang_right=0.0,
ang_left=0.0):
"""One ico for learning to follow a fence.
If it is negative its driving to the right, if its postive it drive to the left.
Args:
reflective ([type]): [description]
predictive ([type]): [description]
Returns:
[type]: [description]
"""
# Run and learning
mc_val = self.ico.run_and_learn(reflective, predictive)
if mc_val < 0.1 and mc_val > -0.1:
right_mc_val = 0.5
left_mc_val = 0.5
else:
right_mc_val = 0.25
left_mc_val = 0.25
left_mc_val += ang_left
right_mc_val += ang_right
if reflective == -1: #(cur_ang < -100) and (reflective == -1): #
# print('right turn')
right_mc_val = 0.0
left_mc_val = 0.2
elif reflective == 1: # (cur_ang > -80) and (reflective == 1):
# print('left turn')
left_mc_val = 0.0
right_mc_val = 0.2
elif mc_val < 0:
left_mc_val += mc_val * (-1)
right_mc_val *= mc_scaling
left_mc_val *= mc_scaling
elif mc_val > 0:
right_mc_val += mc_val
right_mc_val *= mc_scaling
left_mc_val *= mc_scaling
# Upper threshold
if right_mc_val > upper_thresh:
right_mc_val = upper_thresh
if left_mc_val > upper_thresh:
left_mc_val = upper_thresh
if self.log:
cur_time = rospy.get_time()
r_ico_in = predictive
l_ico_in = (-1) * predictive
self.log_ico_one_mc.write_data(self.log_mc_idx, [
cur_time - self.time_log, predictive, self.ico.weight_predic,
mc_val
])
self.log_ico_mc_col.write_data(
self.log_mc_idx, [cur_time - self.time_log, reflective])
self.log_mc_idx += 1
if sim:
lin, ang = self.__convert_to_twist(vel_left=left_mc_val,
vel_right=right_mc_val)
msg = Twist()
msg.linear.x = lin
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = ang
return msg
else:
# Twist
lin, ang = self.__convert_to_twist(vel_left=left_mc_val,
vel_right=right_mc_val,
wheel_dist=39.0)
msg = TwistStamped()
msg.twist.linear.x = lin
msg.twist.linear.y = 0
msg.twist.linear.z = 0
msg.twist.angular.x = 0
msg.twist.angular.y = 0
msg.twist.angular.z = ang
msg.header.stamp = rospy.Time.now()
return msg
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (2 * hdop *
self.alt_std_dev)**2 # FIXME
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# elif 'RMC' in parsed_sentence:
# data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
# if self.use_RMC:
# if data['fix_valid']:
# current_fix.status.status = NavSatStatus.STATUS_FIX
# else:
# current_fix.status.status = NavSatStatus.STATUS_NO_FIX
# current_fix.status.service = NavSatStatus.SERVICE_GPS
# latitude = data['latitude']
# if data['latitude_direction'] == 'S':
# latitude = -latitude
# current_fix.latitude = latitude
# longitude = data['longitude']
# if data['longitude_direction'] == 'W':
# longitude = -longitude
# current_fix.longitude = longitude
# current_fix.altitude = float('NaN')
# current_fix.position_covariance_type = \
# NavSatFix.COVARIANCE_TYPE_UNKNOWN
# self.fix_pub.publish(current_fix)
# if not math.isnan(data['utc_time']):
# current_time_ref.time_ref = rospy.Time.from_sec(
# data['utc_time'])
# self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
self.heading = math.radians(data['heading'])
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
elif 'HDG' in parsed_sentence:
data = parsed_sentence['HDG']
if data['heading']:
magnetic_current_heading = QuaternionStamped()
magnetic_current_heading.header.stamp = current_time
magnetic_current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
magnetic_current_heading.quaternion.x = q[0]
magnetic_current_heading.quaternion.y = q[1]
magnetic_current_heading.quaternion.z = q[2]
magnetic_current_heading.quaternion.w = q[3]
self.magnetic_heading_pub.publish(magnetic_current_heading)
elif 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
if data['speed']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed']
self.vel_pub.publish(current_vel)
self.vel_pub.publish(current_vel)
else:
return False
class VelocityController:
target = PoseStamped()
output = TwistStamped()
def __init__(self):
self.X = PID()
self.Y = PID()
self.Z = PID()
self.lastTime = rospy.get_time()
self.target = None
def setTarget(self, target):
self.target = target
def update(self, state):
if (self.target is None):
rospy.logwarn("Target position for velocity controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
# create output structure
output = TwistStamped()
output.header = state.header
# output velocities
linear = Vector3()
angular = Vector3()
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
# TODO
output.twist = Twist()
output.twist.linear = linear
return output
def stop(self):
setTarget(self.current)
update(self.current)
def set_x_pid(self, kp, ki, kd, output):
self.X.setKp(kp)
self.X.setKi(ki)
self.X.setKd(kd)
self.X.setMaxO(output)
def set_y_pid(self, kp, ki, kd, output):
self.Y.setKp(kp)
self.Y.setKi(ki)
self.Y.setKd(kd)
self.Y.setMaxO(output)
def set_z_pid(self, kp, ki, kd, output):
self.Z.setKp(kp)
self.Z.setKi(ki)
self.Z.setKd(kd)
self.Z.setMaxO(output)
def hook(self):
v = TwistStamped()
v.header.frame_id = '/uav1/base_link_stabilized'
self.vel_pub.publish(v)
def toNED(msg):
# fliter measured velocity
global vx_p, vy_p
vx = 1 * msg.twist.linear.x + 0 * vx_p
vy = 1 * msg.twist.linear.y + 0 * vy_p
v_body = array([vx, -vy, 0])
# transform body velocity to ENU
global q
[qx, qy, qz, qw] = [q[0], q[1], q[2], q[3]]
Tenu = array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw
],
[
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz,
2 * qy * qz - 2 * qx * qw
],
[
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx * qx - 2 * qy * qy
]])
v = dot(Tenu, v_body)
# ENU to NED: (x,y,z) -> (x,-y,-z)
twist = TwistStamped()
twist.header = Header()
twist.header.frame_id = "ned"
twist.header.stamp = rospy.Time.now()
twist.twist.linear.x = v[0]
twist.twist.linear.y = -v[1]
twist.twist.linear.z = 0
pub.publish(twist)
vx_p = vx
vy_p = vy
# record data in vicon frame, compare with vicon
q_ned_vicon = tf.transformations.quaternion_from_euler(math.pi, 0, -yaw)
[qx, qy, qz,
qw] = [q_ned_vicon[0], q_ned_vicon[1], q_ned_vicon[2], q_ned_vicon[3]]
Tv = array([[
1 - 2 * qy * qy - 2 * qz * qz, 2 * qx * qy - 2 * qz * qw,
2 * qx * qz + 2 * qy * qw
],
[
2 * qx * qy + 2 * qz * qw, 1 - 2 * qx * qx - 2 * qz * qz,
2 * qy * qz - 2 * qx * qw
],
[
2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw,
1 - 2 * qx * qx - 2 * qy * qy
]])
vr = dot(Tv, array([v[0], -v[1], 0]))
outtxt.write(str.format("{0:.9f} ", msg.header.stamp.to_sec()))
outtxt.write(str.format("{0:.9f} ", vx))
outtxt.write(str.format("{0:.9f} ", vy))
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0 ')
outtxt.write('0\n')
def run(self):
self.listener.waitForTransform(self.worldFrame, self.frame,
rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = self.worldFrame
circleFlag = 0
acc = AccelStamped()
acc.header.seq = 0
acc.header.frame_id = self.worldFrame
vel = TwistStamped()
vel.header.seq = 0
vel.header.frame_id = self.worldFrame
doCircle = Int8()
doCircle.data = 0
# rate = rospy.Rate(200) # control the publish rate?
while not rospy.is_shutdown():
# hovering at the first given position
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[self.goalIndex][0]
goal.pose.position.y = self.goals[self.goalIndex][1]
goal.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(
0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
acc.header.seq += 1
acc.header.stamp = rospy.Time.now()
acc.accel.linear.z = 0
vel.header.seq += 1
vel.header.stamp = rospy.Time.now()
vel.twist.linear.x = 0
vel.twist.linear.y = 0
self.pubGoal.publish(goal)
self.pubAccel.publish(acc)
self.pubVeloc.publish(vel)
self.pubCircle.publish(doCircle)
# rate.sleep()
t = self.listener.getLatestCommonTime(self.worldFrame, self.frame)
if self.listener.canTransform(self.worldFrame, self.frame, t):
position, quaternion = self.listener.lookupTransform(
self.worldFrame, self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
# if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.1 \
# and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.1 \
# and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.1 \
# and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(5):
if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.1 \
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.1 \
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.1 \
and math.fabs(rpy[2] - 0) < math.radians(5):
rospy.sleep(5)
circleFlag += 1
doCircle.data = 1
t0 = rospy.get_time()
while circleFlag == 1:
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
t = rospy.get_time() - t0
# n = 0.5
# if t > 10 and t <= 20:
# n = 1
# else:
# n = 2
# change the rate
if t > 32:
circleFlag = 2
docircle = 0
# continuously generate the setpoint
goal.pose.position.x = 0.5 * math.cos(math.radians(45 * 2 * t))
goal.pose.position.y = 0.5 * math.sin(math.radians(
45 * 2 * t)) - 0.5
goal.pose.position.z = self.goals[self.goalIndex][2]
x_dt = -0.5 * math.radians(45 * 2) * math.sin(
math.radians(45 * 2 * t))
y_dt = 0.5 * math.radians(45 * 2) * math.cos(
math.radians(45 * 2 * t))
#
x_ddt = -0.5 * math.radians(45 * 2) * math.radians(
45 * 2) * math.cos(math.radians(45 * 2 * t))
y_ddt = -0.5 * math.radians(45 * 2) * math.radians(
45 * 2) * math.sin(math.radians(45 * 2 * t))
# publish x_acc and y_acc
acc.header.seq += 1
acc.header.stamp = rospy.Time.now()
# add scaling factor
acc.accel.linear.x = x_ddt * 180 / math.pi / 9.8
acc.accel.linear.y = y_ddt * 180 / math.pi / 9.8
acc.accel.linear.z = 1
z_ddt = 0
# publish x_vel and y_vel
vel.header.seq += 1
vel.header.stamp = rospy.Time.now()
vel.twist.linear.x = x_dt
vel.twist.linear.y = y_dt
# m = 0.023
########################differential flatness##########################
# g = 9.8
# fzb = np.array([[x_ddt,y_ddt,z_ddt+g]])
# zb = fzb/np.linalg.norm(fzb)
# zbx = zb[0][0]
# zby = zb[0][1]
# zbz = zb[0][2]
# #roll
# roll = math.atan2(zbz,zby)-math.pi/2
# #pitch
# pitch = math.atan2(zbx,zbz)
#######################################################################
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
# acc.accel.angular.x = roll
# acc.accel.angular.y = pitch
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
self.pubAccel.publish(acc)
self.pubVeloc.publish(vel)
self.pubCircle.publish(doCircle)
def two_ico_learning(self,
cur_ang,
reflective,
predictive,
sim: bool = True,
mc_scaling: float = 1.0,
upper_thresh: float = 1.0,
ang_right=0.0,
ang_left=0.0):
"""Two ico's one for each wheel.
If the reflective signal is negative the left motor is learning to drive closer to the fence
If the reflective signal is positive the right motor is learning to drive away from the fence
Args:
reflective ([type]): [description]
predictive ([type]): [description]
Returns:
[type]: [description]
"""
# Run and learning
# Normalize
predictive /= self.learn_inteval
left_mc_val = self.left_ico.run_and_learn(1 if reflective is -1 else 0,
(-1) * predictive)
right_mc_val = self.right_ico.run_and_learn(
1 if reflective is 1 else 0, predictive)
left_mc_val += ang_left
right_mc_val += ang_right
# print(f'Error: {predictive:0.3f}, Decrease left: {ang_left:0.3f}, Decrease Right: {ang_right:0.3f}, Left: {left_mc_val:0.3f}, Right {right_mc_val:0.3f}, Weihgt {self.ico_ang.weight_predic:0.3f}')
if reflective == -1: # (cur_ang < -100) and (reflective == -1): #reflective == -1:
#print('right turn')
right_mc_val = 0.0
left_mc_val = 0.2
elif reflective == 1: # (cur_ang > -80) and (reflective == 1): #reflective == 1:
#print('left turn')
left_mc_val = 0.0
right_mc_val = 0.2
else:
right_mc_val *= mc_scaling
left_mc_val *= mc_scaling
# Upper threshold
if right_mc_val > upper_thresh:
right_mc_val = upper_thresh
if left_mc_val > upper_thresh:
left_mc_val = upper_thresh
if self.log:
cur_time = rospy.get_time()
r_ico_in = predictive
l_ico_in = (-1) * predictive
self.log_ico_two_mc.write_data(self.log_mc_idx, [
cur_time - self.time_log, l_ico_in, r_ico_in,
self.left_ico.weight_predic, self.right_ico.weight_predic,
left_mc_val, right_mc_val
])
self.log_ico_mc_col.write_data(self.log_mc_idx, [
cur_time - self.time_log, 1 if reflective is -1 else 0,
1 if reflective is 1 else 0
])
self.log_mc_idx += 1
if sim:
lin, ang = self.__convert_to_twist(vel_left=left_mc_val,
vel_right=right_mc_val)
msg = Twist()
msg.linear.x = lin
msg.linear.y = 0
msg.linear.z = 0
msg.angular.x = 0
msg.angular.y = 0
msg.angular.z = ang
return msg
else:
lin, ang = self.__convert_to_twist(vel_left=left_mc_val,
vel_right=right_mc_val,
wheel_dist=39.0)
msg = TwistStamped()
msg.twist.linear.x = lin
msg.twist.linear.y = 0
msg.twist.linear.z = 0
msg.twist.angular.x = 0
msg.twist.angular.y = 0
msg.twist.angular.z = ang # / 100 # because not in rad/s on frobit
msg.header.stamp = rospy.Time.now()
return msg
def spin_once(self):
'''Read data from device and publishes ROS messages.'''
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
press_msg = Float32()
pub_press = False
anin1_msg = UInt16()
pub_anin1 = False
anin2_msg = UInt16()
pub_anin2 = False
pub_diag = False
def fill_from_raw(raw_data):
'''Fill messages with information from 'raw' MTData block.'''
# don't publish raw imu data anymore
# TODO find what to do with that
pass
def fill_from_rawgps(rawgps_data):
'''Fill messages with information from 'rawgps' MTData block.'''
global pub_hps, xgps_msg, gps_msg
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
def fill_from_Temp(temp):
'''Fill messages with information from 'temperature' MTData block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = temp
def fill_from_Calib(imu_data):
'''Fill messages with information from 'calibrated' MTData block.'''
global pub_imu, imu_msg, pub_vel, vel_msg, pub_mag, mag_msg
try:
pub_imu = True
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_vel = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
except KeyError:
pass
try:
pub_imu = True
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
except KeyError:
pass
try:
pub_mag = True
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
except KeyError:
pass
def fill_from_Vel(velocity_data):
'''Fill messages with information from 'velocity' MTData block.'''
global pub_vel, vel_msg
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.'''
global pub_imu, imu_msg
pub_imu = True
if orient.has_key('quaternion'):
w, x, y, z = orient['quaternion']
elif orient.has_key('roll'):
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(orient['roll']),
radians(orient['pitch']), radians(orient['yaw']))
elif orient.has_key('matrix'):
m = identity_matrix()
m[:3,:3] = orient['matrix']
x, y, z, w = quaternion_from_matrix(m)
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Pos(position_data):
'''Fill messages with information from 'position' MTData block.'''
global pub_gps, xgps_msg, gps_msg
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
def fill_from_Stat(status):
'''Fill messages with information from 'status' MTData block.'''
global pub_diag, pub_gps, gps_msg, xgps_msg
pub_diag = True
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
def fill_from_Auxiliary(aux_data):
'''Fill messages with information from 'Auxiliary' MTData block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['Ain_1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['Ain_2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Temperature(o):
'''Fill messages with information from 'Temperature' MTData2 block.'''
global pub_temp, temp_msg
pub_temp = True
temp_msg.data = o['Temp']
def fill_from_Timestamp(o):
'''Fill messages with information from 'Timestamp' MTData2 block.'''
global h
try:
# put timestamp from gps UTC time if available
y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
if f&0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO find what to do with other kind of information
pass
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']), radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Pressure(o):
'''Fill messages with information from 'Pressure' MTData2 block.'''
global pub_press, press_msg
press_msg.data = o['Pressure']
def fill_from_Acceleration(o):
'''Fill messages with information from 'Acceleration' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
# FIXME not sure we should treat all in that same way
try:
x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
except KeyError:
pass
try:
x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
except KeyError:
pass
try:
x, y, z = o['accX'], o['accY'], o['accZ']
except KeyError:
pass
imu_msg.linear_acceleration.x = x
imu_msg.linear_acceleration.y = y
imu_msg.linear_acceleration.z = z
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
def fill_from_Position(o):
'''Fill messages with information from 'Position' MTData2 block.'''
global pub_gps, xgps_msg, gps_msg
# TODO publish ECEF
try:
xgps_msg.latitude = gps_msg.latitude = o['lat']
xgps_msg.longitude = gps_msg.longitude = o['lon']
pub_gps = True
alt = o.get('altMsl', o.get('altEllipsoid', 0))
xgps_msg.altitude = gps_msg.altitude = alt
except KeyError:
pass
def fill_from_Angular_Velocity(o):
'''Fill messages with information from 'Angular Velocity' MTData2 block.'''
global pub_imu, imu_msg, pub_vel, vel_msg
try:
imu_msg.angular_velocity.x = o['gyrX']
imu_msg.angular_velocity.y = o['gyrY']
imu_msg.angular_velocity.z = o['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = o['gyrX']
vel_msg.twist.angular.y = o['gyrY']
vel_msg.twist.angular.z = o['gyrZ']
pub_vel = True
except KeyError:
pass
# TODO decide what to do with 'Delta q'
def fill_from_GPS(o):
'''Fill messages with information from 'GPS' MTData2 block.'''
global pub_gps, h, xgps_msg
try: # DOP
xgps_msg.gdop = o['gDOP']
xgps_msg.pdop = o['pDOP']
xgps_msg.hdop = o['hDOP']
xgps_msg.vdop = o['vDOP']
xgps_msg.tdop = o['tDOP']
pub_gps = True
except KeyError:
pass
try: # Time UTC
y, m, d, hr, mi, s, ns, f = o['year'], o['month'], o['day'],\
o['hour'], o['min'], o['sec'], o['nano'], o['valid']
if f&0x4:
secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
h.stamp.secs = secs
h.stamp.nsecs = ns
except KeyError:
pass
# TODO publish SV Info
def fill_from_SCR(o):
'''Fill messages with information from 'SCR' MTData2 block.'''
# TODO that's raw information
pass
def fill_from_Analog_In(o):
'''Fill messages with information from 'Analog In' MTData2 block.'''
global pub_anin1, pub_anin2, anin1_msg, anin2_msg
try:
anin1_msg.data = o['analogIn1']
pub_anin1 = True
except KeyError:
pass
try:
anin2_msg.data = o['analogIn2']
pub_anin2 = True
except KeyError:
pass
def fill_from_Magnetic(o):
'''Fill messages with information from 'Magnetic' MTData2 block.'''
global pub_mag, mag_msg
mag_msg.vector.x = o['magX']
mag_msg.vector.y = o['magY']
mag_msg.vector.z = o['magZ']
pub_mag = True
def fill_from_Velocity(o):
'''Fill messages with information from 'Velocity' MTData2 block.'''
global pub_vel, vel_msg
vel_msg.twist.linear.x = o['velX']
vel_msg.twist.linear.y = o['velY']
vel_msg.twist.linear.z = o['velZ']
pub_vel = True
def fill_from_Status(o):
'''Fill messages with information from 'Status' MTData2 block.'''
try:
status = o['StatusByte']
fill_from_Stat(status)
except KeyError:
pass
try:
status = o['StatusWord']
fill_from_Stat(status)
except KeyError:
pass
# TODO RSSI
def find_handler_name(name):
return "fill_from_%s"%(name.replace(" ", "_"))
# get data
data = self.mt.read_measurement()
# fill messages based on available data fields
for n, o in data:
try:
locals()[find_handler_name(n)](o)
except KeyError:
rospy.logwarn("Unknown MTi data packet: '%s', ignoring."%n)
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
if pub_press:
self.press_pub.publish(press_msg)
if pub_anin1:
self.analog_in1_pub.publish(anin1_msg)
if pub_anin2:
self.analog_in2_pub.publish(anin2_msg)
if pub_diag:
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
# publish string representation
self.str_pub.publish(str(data))
def Driver():
global rate,pub,start_time,states_in, x_est, P_est,eye,Q,Rk,status
if status:
#===============#
# Predict #
#===============#
dy = Quad(0)
xhat = x_est + dy/rate
F = eye + Quad_Jacobian(x_est)/rate
Phat = F*P_est*F.T + Q/rate
New_measurement = CheckInnovation(xhat,states_in,Phat)
if New_measurement: # then use the measurement
#==============#
# Update #
#==============#
deg2rad = np.pi/180
# measurement
zk = np.matrix([[states_in.x],[-states_in.y],[-states_in.z],[states_in.u],[-states_in.v],[-states_in.w],[states_in.phi*deg2rad],[states_in.theta*deg2rad],[states_in.psi*deg2rad],[states_in.p*deg2rad],[states_in.q*deg2rad],[states_in.r*deg2rad]])
# residual/innovation
yk = zk - xhat
# residual/innovation Covariance
Sk = Phat + Rk
# Kalman Gain
Kk = Phat*np.linalg.inv(Sk)
x_est = xhat + Kk*yk
P_est = (eye-Kk)*Phat
else:
x_est = xhat
P_est = Phat
#=================================================#
# Check if Estimate is actually within Room #
#=================================================#
if np.linalg.norm(x_est[0:3]) >6 or (x_est != x_est).all():
x_est = np.asmatrix(np.zeros((12,1)))
x_est[0:3] = np.matrix([[0],[0],[-2]])
P_est = 3*np.asmatrix(np.eye(12))
#--------------#
# Make Message #
#--------------#
cortex = Cortex()
cortex.Obj = [States()]*1
states = States()
states.name = states_in.name
states.visible = states_in.visible
global h
h.seq = h.seq + 1
h.stamp = rospy.Time.now()
h.frame_id = 'cortex'
cortex.header = h
rad2deg = 180/np.pi
# Invert y and z position and velocity from NED to Cartesian
states.x = x_est[0]
states.y = -x_est[1]
states.z = -x_est[2]
states.u = x_est[3]
states.v = -x_est[4]
states.w = -x_est[5]
states.phi = rad2deg*x_est[6]
states.theta = rad2deg*x_est[7]
states.psi = rad2deg*x_est[8]
states.p = rad2deg*x_est[9]
states.q = rad2deg*x_est[10]
states.r = rad2deg*x_est[11]
cortex.Obj[0] = states
global acc
acc.header = h
cov = TwistStamped()
cov.header = h
cov.twist.linear.x = x_est[0]
cov.twist.linear.y = -x_est[1]
cov.twist.linear.z = -x_est[2]
cov.twist.angular.x = P_est[0,0]
cov.twist.angular.y = P_est[1,1]
cov.twist.angular.z = P_est[2,2]
#---------#
# Publish #
#---------#
pub_cov.publish(cov)
pub_acc.publish(acc)
pub.publish(cortex)
def ProcessXlateImage( data ):
global PrevDiameter
global p_x
global p_y
global p_z
global p_z_ang
global FwdCam
global prev_key
global FWD_TAG_DIAMETER
global DWN_TAG_DIAMETER
global TagFound
global NavTwist
global LastTwist
InputTags = data
NewTwist = TwistStamped()
if InputTags.image_width != 320 and FwdCam and prev_key != 'switch':
print "Switch to Down Cam"
p_x.ReInit('vel_x', DWN_X_VEL_SCALAR, DWN_X_INT_SCALAR, DWN_X_DERIVATIVE_SCALAR, DWN_X_MAX_VELOCITY, 0, DWN_X_DEAD_BAND)
p_y.ReInit('vel_y', DWN_Y_VEL_SCALAR, DWN_Y_INT_SCALAR, DWN_Y_DERIVATIVE_SCALAR, DWN_Y_MAX_VELOCITY, 0, DWN_Y_DEAD_BAND)
p_z.ReInit('vel_z', DWN_Z_VEL_SCALAR, DWN_Z_INT_SCALAR, DWN_Z_DERIVATIVE_SCALAR, DWN_Z_MAX_VELOCITY, 0, DWN_Z_DEAD_BAND)
p_z_ang.ReInit('ang_z', DWN_Z_ANG_VEL_SCALAR, DWN_Z_ANG_INT_SCALAR, DWN_Z_ANG_DERIVATIVE_SCALAR, DWN_Z_ANG_MAX_VELOCITY, 0, DWN_Z_ANG_DEAD_BAND)
FwdCam = False
if InputTags.tag_count > 0:
TagFound = True
if InputTags.tags[0].id == 0:
NewTwist.header.frame_id = 'switch'
else:
NewTwist.header.frame_id = 'move'
if FwdCam:
# Forward Camera
# +Z_fwd_cam = +Z_base - points up
# +Y_fwd_cam = +Y_base - points left
# +X_fwd_cam = +X_base - points forward
NewTwist.twist.linear.x = InputTags.tags[0].diameter - FWD_TAG_DIAMETER
NewTwist.twist.linear.y = InputTags.tags[0].x - ( FWD_IMAGE_WIDTH/2 ) #( IMAGE_WIDTH/2 ) - InputTags.tags[0].y
NewTwist.twist.linear.z = InputTags.tags[0].y - ( FWD_IMAGE_HEIGHT/2 ) #( IMAGE_HEIGHT/2 ) - InputTags.tags[0].x
NewTwist.twist.angular.z = 0 # No rotation on fwd cam
#PrevDiameter = InputTags.tags[0].diameter
else:
# Downward Camera
# NOTE: the downward camera
# +Z_down_cam = -Z_base - points down,
# +Y_down_cam = +X_base - points forward
# +X_down_cam = +Y_base - points left
NewTwist.twist.linear.x = InputTags.tags[0].y - ( DWN_IMAGE_HEIGHT/2 )
NewTwist.twist.linear.y = InputTags.tags[0].x - ( DWN_IMAGE_WIDTH/2 )
NewTwist.twist.linear.z = DWN_TAG_DIAMETER - InputTags.tags[0].diameter
print InputTags.tags[0].zRot
NewTwist.twist.angular.z = 2*math.pi - InputTags.tags[0].zRot
#rospy.Publisher("image_pos", NewTwist )
ProcessImagePosition( NewTwist )
# Keep some history for when the tag disappears
while len(PrevVector) >= MAX_HISTORY:
PrevVector.pop(0)
PrevVector.append(NewTwist.twist)
else:
NewTwist.header.frame_id = 'lost'
# Extrapolate history
try:
NewTwist.twist = PrevVector.pop(0)
print "Use History %d" % len(PrevVector)
NewTwist.twist = LastTwist
# Save off some history
except IndexError, e:
# Ran out of history
NewTwist.twist.linear.x = 0
NewTwist.twist.linear.y = 0
NewTwist.twist.linear.z = NavTwist.linear.z
NewTwist.twist.angular.z = 0
if TagFound:
p_x.Reset()
p_y.Reset()
p_z.Reset()
p_z_ang.Reset()
prev_key = 'foobar'
TagFound = False
pub = rospy.Publisher("cmd_vel", Twist )
pub.publish( NewTwist.twist )
"""
import time
from threading import Thread
import math
import matplotlib.pyplot as plt # creation plots
import argparse, sys
import rospy
from geometry_msgs.msg import TwistStamped
from rc_bringup.msg import CarPwmContol
pwm = CarPwmContol()
pwm.MotorPWM = 1550
vel = TwistStamped()
vel_norm = float()
cmd_vel_topic = "/rc_car/velocity" # output topic
pwm_topic = "rc_car/pwm"
class PwmThread(Thread):
"""
A threading example
"""
def __init__(self, name):
"""Инициализация потока"""
Thread.__init__(self)
self.name = name
from tf2_msgs.msg import TFMessage
import tf
import numpy as np
from mav_msgs.msg import RollPitchYawrateThrust
model = "/f450/"
odom = Odometry()
def odom_cb(data):
global odom
odom = data
vrpn = TwistStamped()
def vrpn_cb(data):
global vrpn
vrpn = data
rpyth = RollPitchYawrateThrust()
def rpyth_cb(data):
global rpyth
rpyth = data
def __init__(self):
### define gym spaces ###
self.min_action = 0.0
self.max_action = 1.0
self.min_position = 0.1
self.max_position = 25
self.max_speed = 3
self.low_state = np.array([self.min_position, -self.max_speed])
self.high_state = np.array([self.max_position, self.max_speed])
# self.low_state = np.array([self.min_position])
# self.high_state = np.array([self.max_position])
self.action_space = spaces.Box(low=self.min_action,
high=self.max_action,
shape=(1, ),
dtype=np.float32)
self.observation_space = spaces.Box(low=self.low_state,
high=self.high_state,
dtype=np.float32)
self.current_state = State()
self.imu_data = Imu()
self.act_controls = ActuatorControl()
self.pose = PoseStamped()
self.mocap_pose = PoseStamped()
self.thrust_ctrl = Thrust()
self.attitude_target = AttitudeTarget()
self.local_velocity = TwistStamped()
self.global_velocity = TwistStamped()
### define ROS messages ###
self.offb_set_mode = SetModeRequest()
self.offb_set_mode.custom_mode = "OFFBOARD"
self.arm_cmd = CommandBoolRequest()
self.arm_cmd.value = True
self.disarm_cmd = CommandBoolRequest()
self.disarm_cmd.value = False
## ROS Subscribers
self.state_sub = rospy.Subscriber("/mavros/state",
State,
self.state_cb,
queue_size=10)
self.imu_sub = rospy.Subscriber("/mavros/imu/data",
Imu,
self.imu_cb,
queue_size=10)
self.local_pos_sub = rospy.Subscriber("/mavros/local_position/pose",
PoseStamped,
self.lp_cb,
queue_size=10)
self.local_vel_sub = rospy.Subscriber(
"/mavros/local_position/velocity_local",
TwistStamped,
self.lv_cb,
queue_size=10)
self.act_control_sub = rospy.Subscriber(
"/mavros/act_control/act_control_pub",
ActuatorControl,
self.act_cb,
queue_size=10)
self.global_alt_sub = rospy.Subscriber(
"/mavros/global_position/rel_alt",
Float64,
self.ra_cb,
queue_size=10)
self.global_pos_sub = rospy.Subscriber(
"/mavros/global_position/gp_vel",
TwistStamped,
self.gv_cb,
queue_size=10)
## ROS Publishers
self.local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
self.mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",
PoseStamped,
queue_size=10)
self.acutator_control_pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
self.setpoint_raw_pub = rospy.Publisher(
"/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10)
self.thrust_pub = rospy.Publisher("/mavros/setpoint_attitude/thrust",
Thrust,
queue_size=10)
## initiate gym enviornment
self.init_env()
## ROS Services
rospy.wait_for_service('mavros/cmd/arming')
self.arming_client = rospy.ServiceProxy('mavros/cmd/arming',
CommandBool)
rospy.wait_for_service('mavros/set_mode')
self.set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.wait_for_service('mavros/set_stream_rate')
set_stream_rate = rospy.ServiceProxy("mavros/set_stream_rate",
StreamRate)
set_stream_rate(StreamRateRequest.STREAM_POSITION, 50, 1)
set_stream_rate(StreamRateRequest.STREAM_ALL, 50, 1)
self.setpoint_msg = mavros.setpoint.PoseStamped(
header=mavros.setpoint.Header(frame_id="att_pose",
stamp=rospy.Time.now()), )
self.offb_arm()
#!/usr/bin/env python
import rospy
from mavros_msgs.srv import SetMode, CommandBool, CommandTOL
from geometry_msgs.msg import TwistStamped, Vector3
import time
import pygame
import pygame.locals
rospy.init_node('mavros_final_project')
rate = rospy.Rate(10)
commandVelocityPub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
setVelocity = TwistStamped()
def setMode():
print("Setting Mode to Guided: mode guided")
rospy.wait_for_service('/mavros/set_mode')
try:
mavSetMode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
mavSetMode(custom_mode="Guided")
except rospy.ServiceException as e:
print(e)
def armCopter():
print("Arming throttle: arm throttle")
rospy.wait_for_service('/mavros/cmd/arming')
def trajectory_controller(self, path):
count = 0
previous_index = 0
path_x = path[0, :]
path_y = path[1, :]
path_z = path[2, :]
while True:
try:
(trans, rot) = self.tf_listener.lookupTransform(
'/base_link', '/arm_link_5', rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
current_pos = np.array([trans[0], trans[1], trans[2]])
distance = np.linalg.norm(
(np.array(path[:, path.shape[1] - 1]) - current_pos))
print "final pos is ", path[:, path.shape[1] - 1]
while distance > self.goal_tolerance and self.event != 'e_stop' and not rospy.is_shutdown(
):
try:
(trans, rot) = self.tf_listener.lookupTransform(
'/base_link', '/arm_link_5', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
message = TwistStamped()
current_pos = np.array([trans[0], trans[1], trans[2]])
distance = np.linalg.norm(
(np.array(path[:, path.shape[1] - 1]) - current_pos))
dist = []
for i in range(path.shape[1]):
dist.append(np.linalg.norm((path[:, i] - current_pos)))
index = np.argmin(dist)
if index < previous_index:
index = previous_index
else:
previous_index = index
# Delete this block later
if (index > path.shape[1] - 1):
break
# Check if path repeated points in it, this will prevent trajectory execution as velocity between these two points will
# be zero (difference is zero)
if index == path.shape[1] - 1:
ind = index
else:
ind = index + 1
# print index, ind, path.shape[1]
vel_x = self.feedforward_gain * (
path_x[ind] - path_x[index]) + self.feedback_gain * (
path_x[ind] - current_pos[0])
vel_y = self.feedforward_gain * (
path_y[ind] - path_y[index]) + self.feedback_gain * (
path_y[ind] - current_pos[1])
vel_z = self.feedforward_gain * (
path_z[ind] - path_z[index]) + self.feedback_gain * (
path_z[ind] - current_pos[2])
message.header.seq = count
message.header.frame_id = "/base_link"
message.twist.linear.x = vel_x
message.twist.linear.y = vel_y
message.twist.linear.z = vel_z
self.vel_publisher.publish(message)
count += 1
message = TwistStamped()
message.header.seq = count
message.header.frame_id = "/base_link"
message.twist.linear.x = 0.0
message.twist.linear.y = 0.0
message.twist.linear.z = 0.0
self.vel_publisher.publish(message)
'box' in data.states[i].collision1_name
): # check that the string exist in collision2_name/1
depth = np.mean(data.states[i].depths)
z_collision = np.mean(data.states[i].contact_positions[0].z)
def odom_callback(data):
global odom
odom = data
if __name__ == '__main__':
rospy.init_node('movingbobcat_gym', anonymous=True, log_level=rospy.WARN)
global depth, command, odom, pub2
command = TwistStamped()
depth = 0
rospy.wait_for_service('/gazebo/get_model_state')
get_model_state = rospy.ServiceProxy("/gazebo/get_model_state",
GetModelState)
rospy.wait_for_service('/gazebo/get_link_state')
get_link_state = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState)
pub = rospy.Publisher('/WPD/Speed', TwistStamped, queue_size=10)
pub2 = rospy.Publisher('/bobcat/arm/hydraulics', Float64, queue_size=10)
pub3 = rospy.Publisher('/bobcat/arm/loader', Float64, queue_size=10)
rospy.Subscriber('/robot_bumper', ContactsState, get_depth)
rospy.Subscriber("/Bobby/odom", Odometry, odom_callback)
# Create the Gym environment
env = gym.make('MovingBobcat-v0')
print("Start------------------------------------")
def __init__(self):
# fixed parameters
self.update_rate = 20 # [Hz]
self.vel_lin_user_max = 0.5
self.vel_ang_user_max = 0.3
# robot state
self.STATE_AUTO = 0
self.STATE_MANUAL = 1
self.state = self.STATE_MANUAL
# wiimote state
self.wii_1 = False
self.wii_1_changed = False
self.wii_2 = False
self.wii_2_changed = False
self.wii_a = False
self.wii_a_changed = False
self.wii_b = False
self.wii_b_changed = False
self.wii_plus = False
self.wii_plus_changed = False
self.wii_minus = False
self.wii_minus_changed = False
self.wii_up = False
self.wii_up_changed = False
self.wii_down = False
self.wii_down_changed = False
self.wii_left = False
self.wii_left_changed = False
self.wii_right = False
self.wii_right_changed = False
self.wii_home = False
self.wii_home_changed = False
self.wii_angle_min = 3.0 * pi / 180.0
self.wii_angle_max = 60.0 * pi / 180.0
self.wii_angle_diff = self.wii_angle_max - self.wii_angle_min
# read parameters
self.vel_lin_max = rospy.get_param("~max_linear_velocity",
1.0) # [m/s]
self.vel_ang_max = rospy.get_param("~max_angular_velocity",
0.5) # [rad/s]
acc_lin_max = rospy.get_param("~max_linear_acceleration",
2.0) # [m/s^2]
acc_ang_max = rospy.get_param("~max_angular_acceleration",
pi) # [rad/s^2]
self.acc_lin_max_step = acc_lin_max / (self.update_rate * 1.0)
self.acc_ang_max_step = acc_ang_max / (self.update_rate * 1.0)
dec_lin_max = rospy.get_param("~max_linear_deceleration",
2.0) # [m/s^2]
dec_ang_max = rospy.get_param("~max_angular_deceleration",
pi) # [rad/s^2]
self.dec_lin_max_step = dec_lin_max / (self.update_rate * 1.0)
self.dec_ang_max_step = dec_ang_max / (self.update_rate * 1.0)
# get topic names
joy_topic = rospy.get_param("~wiimote_sub", '/fmLib/joy')
deadman_topic = rospy.get_param("~deadman_pub", "/fmCommand/deadman")
automode_topic = rospy.get_param("~automode_pub",
"/fmDecision/automode")
cmd_vel_topic = rospy.get_param("~cmd_vel_pub", "/fmCommand/cmd_vel")
# setup deadman publish topic
self.deadman_state = False
self.deadman_msg = Bool()
self.deadman_pub = rospy.Publisher(deadman_topic, Bool)
# setup automode publish topic
self.automode_msg = Bool()
self.automode_pub = rospy.Publisher(automode_topic, Bool)
# setup manual velocity topic
self.vel_lin_user = 0.0
self.vel_ang_user = 0.0
self.vel_lin = 0.0
self.vel_ang = 0.0
self.cmd_vel_msg = TwistStamped()
self.cmd_vel_pub = rospy.Publisher(cmd_vel_topic, TwistStamped)
# setup subscription topic callbacks
rospy.Subscriber(joy_topic, Joy, self.on_joy_topic)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def get_velocity(self, pose): velocity = TwistStamped() velocity.header.stamp = pose.header.stamp velocity.header.frame_id = pose.header.frame_id velocity.twist = pose.twist.twist return velocity
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
data = parsed_sentence['GGA']
gps_qual = data['fix_type']
if gps_qual == 0:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
elif gps_qual == 1:
current_fix.status.status = NavSatStatus.STATUS_FIX
elif gps_qual == 2:
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
elif gps_qual in (4, 5):
current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX
elif gps_qual == 9:
# Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9
# http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/
current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
current_fix.header.stamp = current_time
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
hdop = data['hdop']
current_fix.position_covariance[0] = hdop**2
current_fix.position_covariance[4] = hdop**2
current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
else:
return False
def send_cmd(self, linear, angular):
ts = TwistStamped()
ts.header.stamp = rospy.Time.now()
ts.twist.linear.x, ts.twist.linear.y, ts.twist.linear.z = linear
ts.twist.angular.x, ts.twist.angular.y, ts.twist.angular.z = angular
self._pub.publish(ts)
def copy_vel(self, vel):
copied_vel = TwistStamped()
copied_vel.header= vel.header
return copied_vel
def __init__(self):
self.update_rate = 20 # [Hz]
# robot state
self.STATE_AUTO = 0
self.STATE_MANUAL = 1
self.state = self.STATE_MANUAL
# HMI id's
self.HMI_ID_DEADMAN = 0
self.HMI_ID_MODE = 1
self.HMI_ID_GOTO_WAYPOINT = 2
self.HMI_MODE_MANUAL = 0
self.HMI_MODE_AUTO = 1
# keyboard interface
self.KEY_ESC = 27
self.KEY_SECOND = 91
self.KEY_SPACE = 32
self.KEY_a = 97
self.KEY_e = 101
self.KEY_m = 109
self.KEY_s = 115
self.KEY_ARROW_UP = 65
self.KEY_ARROW_DOWN = 66
self.KEY_ARROW_RIGHT = 67
self.KEY_ARROW_LEFT = 68
self.esc_key = False
self.second_key = False
# read parameters
self.vel_lin_max = rospy.get_param("~max_linear_velocity",
0.5) # [m/s]
self.vel_ang_max = rospy.get_param("~max_angular_velocity",
0.3) # [rad/s]
self.vel_lin_step = rospy.get_param("~linear_velocity_step",
0.1) # [m/s]
self.vel_ang_step = rospy.get_param("~angular_velocity_step",
0.1) # [rad/s]
# get topic names
kbd_topic = rospy.get_param("~keyboard_sub", "/fmHMI/keyboard")
deadman_topic = rospy.get_param("~deadman_pub", "/fmCommand/deadman")
hmi_pub_topic = rospy.get_param("~hmi_pub", '/fmDecision/hmi')
automode_topic = rospy.get_param("~automode_pub",
"/fmDecision/automode")
cmd_vel_topic = rospy.get_param("~cmd_vel_pub", "/fmCommand/cmd_vel")
# setup deadman publish topic
self.deadman_state = False
self.deadman_msg = Bool()
self.deadman_pub = rospy.Publisher(deadman_topic, Bool)
# setup automode publish topic
self.automode_msg = Bool()
self.automode_pub = rospy.Publisher(automode_topic, Bool)
# setup HMI publish topic
self.hmi_msg = StringArrayStamped()
self.hmi_msg.data = ['', ''] # initialize string array
self.hmi_pub = rospy.Publisher(hmi_pub_topic, StringArrayStamped)
# setup manual velocity topic
self.vel_lin = 0.0
self.vel_ang = 0.0
self.cmd_vel_msg = TwistStamped()
self.cmd_vel_pub = rospy.Publisher(cmd_vel_topic, TwistStamped)
# setup subscription topic callbacks
rospy.Subscriber(kbd_topic, Char, self.on_kbd_topic)
# sall updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
def get_desired_command(self):
with self._lock:
if self._canceled:
return (TaskCanceled(), )
if (self._state == BlockRoombaTaskState.init):
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()
or not self.topic_buffer.has_landing_message()):
self._state = BlockRoombaTaskState.waiting
else:
self._state = BlockRoombaTaskState.descent
if (self._state == BlockRoombaTaskState.waiting):
if (not self.topic_buffer.has_roomba_message()
or not self.topic_buffer.has_odometry_message()
or not self.topic_buffer.has_landing_message()):
return (TaskRunning(), NopCommand())
else:
self._state = BlockRoombaTaskState.descent
if (self._state == BlockRoombaTaskState.descent):
try:
roomba_transform = self.topic_buffer.get_tf_buffer(
).lookup_transform('level_quad', self._roomba_id,
rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
drone_transform = self.topic_buffer.get_tf_buffer(
).lookup_transform('level_quad', 'map', rospy.Time(0),
rospy.Duration(self._TRANSFORM_TIMEOUT))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as ex:
rospy.logerr(
'Block Roomba Task: Exception when looking up transform'
)
rospy.logerr(ex.message)
return (TaskAborted(
msg=
'Exception when looking up transform during block roomba'
), )
# Creat point centered at drone's center
stamped_point = PointStamped()
stamped_point.point.x = 0
stamped_point.point.y = 0
stamped_point.point.z = 0
# returns point distances of roomba to center point of level quad
self._roomba_point = tf2_geometry_msgs.do_transform_point(
stamped_point, roomba_transform)
if not self._check_max_roomba_range():
return (TaskAborted(
msg=
'The provided roomba is not close enough to the quad'),
)
if self._on_ground():
return (TaskDone(), )
roomba_x_velocity = self._roomba_odometry.twist.twist.linear.x
roomba_y_velocity = self._roomba_odometry.twist.twist.linear.y
roomba_velocity = math.sqrt(roomba_x_velocity**2 +
roomba_y_velocity**2)
roomba_vector = Vector3Stamped()
roomba_vector.vector.x = roomba_x_velocity / roomba_velocity
roomba_vector.vector.y = roomba_y_velocity / roomba_velocity
roomba_vector.vector.z = 0.0
# p-controller
x_vel_target = (
(self._roomba_point.point.x +
self._overshoot * roomba_vector.vector.x) * self._K_X +
roomba_x_velocity)
y_vel_target = (
(self._roomba_point.point.y +
self._overshoot * roomba_vector.vector.y) * self._K_Y +
roomba_y_velocity)
z_vel_target = self._descent_velocity
#caps velocity
vel_target = math.sqrt(x_vel_target**2 + y_vel_target**2)
if vel_target > self._MAX_TRANSLATION_SPEED:
x_vel_target = x_vel_target * (
self._MAX_TRANSLATION_SPEED / vel_target)
y_vel_target = y_vel_target * (
self._MAX_TRANSLATION_SPEED / vel_target)
if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
z_vel_target = z_vel_target / abs(
z_vel_target) * self._MAX_Z_VELOCITY
rospy.logwarn("Max Z velocity reached in block roomba")
desired_vel = [x_vel_target, y_vel_target, z_vel_target]
odometry = self.topic_buffer.get_odometry_message()
drone_vel_x = odometry.twist.twist.linear.x
drone_vel_y = odometry.twist.twist.linear.y
drone_vel_z = odometry.twist.twist.linear.z
if self._current_velocity is None:
self._current_velocity = [
drone_vel_x, drone_vel_y, drone_vel_z
]
desired_vel = self._limiter.limit_acceleration(
self._current_velocity, desired_vel)
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
velocity.twist.linear.x = desired_vel[0]
velocity.twist.linear.y = desired_vel[1]
velocity.twist.linear.z = desired_vel[2]
self._current_velocity = desired_vel
return (TaskRunning(), VelocityCommand(velocity))
return (TaskAborted(
msg='Illegal state reached in Block Roomba Task'), )
def main():
# INITIALIZE ADAPTIVE NETWORK PARAMETERS:
N_INPUT = 4 # Number of input network
N_OUTPUT = 1 # Number of output network
# INIT_NODE = 1 # Number of node(s) for initial structure
# INIT_LAYER = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
INIT_NODE_X = 1 # Number of node(s) for initial structure
INIT_LAYER_X = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
INIT_NODE_Y = 1 # Number of node(s) for initial structure
INIT_LAYER_Y = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
INIT_NODE_Z = 1 # Number of node(s) for initial structure
INIT_LAYER_Z = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
N_WINDOW = 300 # Sliding Window Size
EVAL_WINDOW = 3 # Evaluation Window for layer Adaptation
DELTA = 0.05 # Confidence Level for layer Adaptation (0.05 = 95%)
ETA = 0.0001 # Minimum allowable value if divided by zero
FORGET_FACTOR = 0.98 # Forgetting Factor of Recursive Calculation
#EXP_DECAY = 1 # Learning Rate decay factor
#LEARNING_RATE = 0.00004 # Network optimization learning rate
#LEARNING_RATE = 0.01 # Network optimization learning rate
# LEARNING_RATE_X = 0.02 # Network optimization learning rate 0.00085
# LEARNING_RATE_Y = 0.02 # Network optimization learning rate 0.00085
# LEARNING_RATE_Z = 0.05 # Network optimization learning rate
LEARNING_RATE_X = 0.0125 # Network optimization learning rate 0.00085
LEARNING_RATE_Y = 0.0125 # Network optimization learning rate 0.00085
LEARNING_RATE_Z = 0.06 # Network optimization learning rate
WEIGHT_DECAY = 0.000125 # Regularization weight decay factor
#WEIGHT_DECAY = 0.0 # Regularization weight decay factor
#SLIDING_WINDOW = 35
# INITIALIZE SYSTEM AND SIMULATION PARAMETERS:
IRIS_THRUST = 0.5629 # Nominal thrust for IRIS Quadrotor
RATE = 25.0 # Sampling Frequency (Hz)
# PID_GAIN_X = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-X
# PID_GAIN_Y = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-Y
# PID_GAIN_X = [0.45,0.0,0.002] # PID Gain Parameter [KP,KI,KD] axis-X
# PID_GAIN_Y = [0.45,0.0,0.002] # PID Gain Parameter [KP,KI,KD] axis-Y
PID_GAIN_X = [0.45, 0.0, 0.0] # PID Gain Parameter [KP,KI,KD] axis-X
PID_GAIN_Y = [0.45, 0.0, 0.0] # PID Gain Parameter [KP,KI,KD] axis-Y
#PID_GAIN_Z = [0.013,0.0004,0.2] # PID Gain Parameter [KP,KI,KD] axis-Z
# PID_GAIN_Z = [0.85,0.0,0.0001] # PID Gain Parameter [KP,KI,KD] axis-Z
PID_GAIN_Z = [2.25, 0.0, 0.0] # PID Gain Parameter [KP,KI,KD] axis-Z
SIM_TIME = 295 # Simulation time duration (s)
#--------------------------------------------------------------------------------
# Initial conditions of UAV system
xref = 0.0
yref = 0.0
zref = 1.0
interrx = 0.0
interry = 0.0
interrz = 0.0
errlastx = 0.0
errlasty = 0.0
errlastz = 0.0
runtime = 0.0
# Ignite the Evolving NN Controller
Xcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE_X)
Ycon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE_Y)
Zcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE_Z)
if INIT_LAYER_X > 2:
for i in range(INIT_LAYER_X - 2):
Xcon.addhiddenlayer()
if INIT_LAYER_Y > 2:
for i in range(INIT_LAYER_Y - 2):
Ycon.addhiddenlayer()
if INIT_LAYER_Z > 2:
for i in range(INIT_LAYER_Z - 2):
Zcon.addhiddenlayer()
dt = 1 / RATE
Xcon.dt = dt
Ycon.dt = dt
Zcon.dt = dt
Xcon.smcpar = PID_GAIN_X
Ycon.smcpar = PID_GAIN_Y
Zcon.smcpar = PID_GAIN_Z
#Init Weight
# Wx0=torch.tensor([[ 0.3051, 0.3059, 0.3048, -0.0287],
# [ 0.3099, 0.3084, 0.3077, -0.0291],
# [ 0.3097, 0.3088, 0.3063, -0.0300]], dtype=torch.float64,requires_grad = True)
# Wx1=torch.tensor([[0.4430, 0.4474, 0.4469],
# [0.3885, 0.3906, 0.3904],
# [0.3092, 0.3079, 0.3083]], dtype=torch.float64,requires_grad = True)
# Wx2=torch.tensor([[0.7390, 0.5607, 0.3045]], dtype=torch.float64,requires_grad = True)
# Wy0=torch.tensor([[-0.0770, -0.0733, -0.0730, 0.0026],
# [-0.3706, -0.3641, -0.3646, 0.0189]], dtype=torch.float64,requires_grad = True)
# Wy1=torch.tensor([[-0.2303, -1.1563]], dtype=torch.float64,requires_grad = True)
# # Wz0=torch.tensor([[-0.3250, -0.3174, -0.3083, 0.0508]], dtype=torch.float64,requires_grad = True)
# # Wz1=torch.tensor([[-0.5591]], dtype=torch.float64,requires_grad = True)
# Xcon.netStruct[0].linear.weight.data = Wx0
# Xcon.netStruct[1].linear.weight.data = Wx1
# Xcon.netStruct[2].linear.weight.data = Wx2
# Ycon.netStruct[0].linear.weight.data = Wy0
# Ycon.netStruct[1].linear.weight.data = Wy1
# Zcon.netStruct[0].linear.weight.data = Wz0
# Zcon.netStruct[1].linear.weight.data = Wz1
# PX4 API object
modes = fcuModes() # Flight mode object
uav = uavCommand() # UAV command object
trajectory = Trajectory()
meanErrX = recursiveMean()
meanErrY = recursiveMean()
meanErrZ = recursiveMean()
# Data Logger object
logData = dataLogger('flight')
xconLog = dataLogger('X')
yconLog = dataLogger('Y')
zconLog = dataLogger('Z')
# Initiate node and subscriber
rospy.init_node('setpoint_node', anonymous=True)
rate = rospy.Rate(RATE) # ROS loop rate, [Hz]
# Subscribe to drone state
rospy.Subscriber('/mavros/state', State, uav.stateCb)
# Subscribe to drone's local position
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, uav.posCb)
# Setpoint publisher
velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
# Velocity messages init
vel = TwistStamped()
vel.twist.linear.x = 0.0
vel.twist.linear.y = 0.0
vel.twist.linear.z = 0.0
# Arming the UAV --> no auto arming, please comment out line 140
text = colored("Ready for Flight..Press Enter to continue...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
k = 0
while not uav.state.armed:
#modes.setArm()
rate.sleep()
k += 1
if k % 10 == 0:
text = colored('Waiting for arming..', 'yellow')
print(text)
if k > 500:
text = colored('Arming timeout..',
'red',
attrs=['reverse', 'blink'])
print(text)
break
# Switch to OFFBOARD after send few setpoint messages
text = colored("Vehicle Armed!! Press Enter to switch OFFBOARD...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
# while not uav.state.armed:
# #modes.setArm()
# rate.sleep()
# text = colored('Vehicle is not arming..', 'yellow')
# print (text)
rate.sleep()
k = 0
while k < 10:
velocity_pub.publish(vel)
rate.sleep()
k = k + 1
modes.setOffboardMode()
text = colored('Now in OFFBOARD Mode..',
'blue',
attrs=['reverse', 'blink'])
print(text)
# ROS Main loop::
k = 0
while not rospy.is_shutdown():
start = time.time()
rate.sleep()
# Setpoint generator:
# Take off with altitude Z = 1 m
if runtime <= 15:
xref = 0.0
yref = 0.0
zref = 0.3 + trajectory.linear(0.7, dt, 15.0)
if runtime == 15:
trajectory.reset()
j = 0
xs = uav.local_pos.x
xr = np.linspace(xs, -0.9, num=(5.0 / dt))
# Go to X=-0.9 direction
if runtime > 15 and runtime <= 20:
xref = xr[j]
yref = 0.0
zref = 1.0
j += 1
if runtime == 20:
trajectory.reset()
ys = uav.local_pos.y
j = 0
yr = np.linspace(ys, 1.0, num=(5.0 / dt))
# Go to Y =1 direction
if runtime > 20 and runtime <= 25:
xref = -0.9
yref = yr[j]
zref = 1.0
j += 1
if runtime == 25:
trajectory.reset()
zs = uav.local_pos.z
zr = np.linspace(zs, 1.3, num=(5.0 / dt))
j = 0
# Go to Z= 1.3
if runtime > 25 and runtime <= 35:
xref = -0.9
yref = 1.0
# zref = zr[j]
zref = 1.0
j += 1
if runtime == 30:
trajectory.reset()
j = 0
# Hold 5 s
# if runtime > 30 and runtime <=35:
# xref = -0.9
# yref = 1.0
# zref = 1.3
# if runtime == 35:
# trajectory.reset()
# j = 0
# Sinusoidal Z trajectory after 10 times
if runtime > 35 and runtime <= 185:
xref = -0.9
yref = 1.0
zref = 1.0 + trajectory.sinusoid(0.5, dt, 15)
if runtime == 185:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 1.0, num=(5.0 / dt))
# Go to Z=1.0
if runtime > 185 and runtime <= 190:
xref = -0.9
yref = 1.0
zref = zr[j]
j += 1
if runtime == 190:
trajectory.reset()
xs = uav.local_pos.x
ys = uav.local_pos.y
j = 0
yr = np.linspace(ys, -1.67, num=(10.0 / dt))
# Go to Y=-1.65
if runtime > 190 and runtime <= 200:
xref = -0.9
yref = yr[j]
zref = 1.0
j += 1
if runtime == 200:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 1.3, num=(5.0 / dt))
# Go to Z= 1.3
if runtime > 200 and runtime <= 210:
xref = -0.9
yref = -1.67
# zref = zr[j]
zref = 1.0
j += 1
if runtime == 210:
trajectory.reset()
j = 0
# Hold 5 s
# if runtime > 205 and runtime <=210:
# xref = -0.9
# yref = -1.67
# zref = 1.3
# if runtime == 205:
# trajectory.reset()
# j = 0
# Approach Ceiling +- 0.07 4 times
if runtime > 210 and runtime <= 275:
xref = -0.9
yref = -1.67
zref = 1.0 + trajectory.pulse(0.25, dt, 15)
if runtime == 275:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 1.0, num=(5.0 / dt))
# Go to Z = 1.0
if runtime > 275 and runtime <= 280:
xref = -0.9
yref = -1.67
zref = zr[j]
j += 1
if runtime == 280:
trajectory.reset()
ys = uav.local_pos.y
j = 0
yr = np.linspace(ys, 1.0, num=(5.0 / dt))
# Go to Y= 1
if runtime > 280 and runtime <= 285:
xref = -0.9
yref = yr[j]
zref = 1.0
j += 1
if runtime == 285:
trajectory.reset()
zs = uav.local_pos.z
j = 0
zr = np.linspace(zs, 0.4, num=(10.0 / dt))
#Landing
if runtime > 285:
xref = -0.9
yref = 1.0
zref = zr[j]
j += 1
# update current position
xpos = uav.local_pos.x
ypos = uav.local_pos.y
zpos = uav.local_pos.z
# calculate errors,delta errors, and integral errors
errx, derrx, interrx = Xcon.calculateError(xref, xpos)
erry, derry, interry = Ycon.calculateError(yref, ypos)
errz, derrz, interrz = Zcon.calculateError(zref, zpos)
# PID Controller equations
#theta_ref = 0.04*errx+0.0005*interrx+0.01*derrx
#phi_ref = 0.04*erry+0.0005*interry+0.01*derry
# vx = PID_GAIN_X[0]*errx+PID_GAIN_X[1]*interrx+PID_GAIN_X[2]*derrx
# vy = PID_GAIN_Y[0]*erry+PID_GAIN_Y[1]*interry+PID_GAIN_Y[2]*derry
#thrust_ref = IRIS_THRUST+PID_GAIN_Z[0]*errz+PID_GAIN_Z[1]*interrz+PID_GAIN_Z[2]*derrz
# SMC + NN controller
vx = Xcon.controlUpdate(xref) # Velocity X
vy = Ycon.controlUpdate(yref) # Velocity Y
vz = Zcon.controlUpdate(zref) # Additional Thrust Z
vx = limiter(vx, 1)
vy = limiter(vy, 1)
vz = limiter(vz, 1)
# Publish the control signal
vel.twist.linear.x = vx
vel.twist.linear.y = vy
vel.twist.linear.z = vz
velocity_pub.publish(vel)
# NN Learning stage
# if meanErrX.updateMean(abs(errx),0.99) > 0.05:
# Xcon.optimize(LEARNING_RATE_X,WEIGHT_DECAY)
# if meanErrY.updateMean(abs(erry),0.99) > 0.05:
# Ycon.optimize(LEARNING_RATE_Y,WEIGHT_DECAY)
# if meanErrZ.updateMean(abs(errz),0.99) > 0.05:
# Zcon.optimize(LEARNING_RATE_Z,WEIGHT_DECAY)
Xcon.optimize(LEARNING_RATE_X, WEIGHT_DECAY)
Ycon.optimize(LEARNING_RATE_Y, WEIGHT_DECAY)
Zcon.optimize(LEARNING_RATE_Z, WEIGHT_DECAY)
# Adjust the number of nodes in the latest layer (Network Width)
Xcon.adjustWidth(FORGET_FACTOR, N_WINDOW)
Ycon.adjustWidth(FORGET_FACTOR, N_WINDOW)
Zcon.adjustWidth(FORGET_FACTOR, N_WINDOW)
# # Adjust the number of layer (Network Depth)
Xcon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
Ycon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
Zcon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
#euler = euler_from_quaternion(uav.q)
# Print all states
print('Xr,Yr,Zr = ', xref, yref, zref)
print('X, Y, Z = ', uav.local_pos.x, uav.local_pos.y,
uav.local_pos.z)
print('ex, ey, ez = ', errx, erry, errz)
print('vx,vy,vz = ', vx, vy, vz)
#print 'att angles = ',euler
print('Nodes X Y Z = ', Xcon.nNodes, Ycon.nNodes, Zcon.nNodes)
print('Layer X Y Z = ', Xcon.nLayer, Ycon.nLayer, Zcon.nLayer)
# print Ycon.netStruct[0].linear.weight.data
# print Ycon.netStruct[1].linear.weight.data
print('')
k += 1
runtime = k * dt
execTime = time.time() - start
print('Runtime = ', runtime)
print('Exec time = ', execTime)
print('Sim time = ', SIM_TIME)
print('')
# Append logged Data
logData.appendStateData(runtime, execTime, xref, yref, zref, xpos,
ypos, zpos, vx, vy, vz)
xconLog.appendControlData(runtime, Xcon.us, Xcon.un, Xcon.nLayer,
Xcon.nNodes)
yconLog.appendControlData(runtime, Ycon.us, Ycon.un, Ycon.nLayer,
Ycon.nNodes)
zconLog.appendControlData(runtime, Zcon.us, Zcon.un, Zcon.nLayer,
Zcon.nNodes)
# Save logged Data
logData.saveStateData(runtime, execTime, xref, yref, zref, xpos, ypos,
zpos, vx, vy, vz)
xconLog.saveControlData(runtime, Xcon.us, Xcon.un, Xcon.nLayer,
Xcon.nNodes)
yconLog.saveControlData(runtime, Ycon.us, Ycon.un, Ycon.nLayer,
Ycon.nNodes)
zconLog.saveControlData(runtime, Zcon.us, Zcon.un, Zcon.nLayer,
Zcon.nNodes)
# Break condition
if runtime > SIM_TIME:
# Set auto land mode
modes.setRTLMode()
text = colored('Auto landing mode now..',
'blue',
attrs=['reverse', 'blink'])
print(text)
text = colored('Now saving all logged data..',
'yellow',
attrs=['reverse', 'blink'])
print(text)
# Closing the saved log files
logData.logFile.close()
xconLog.logFile.close()
yconLog.logFile.close()
zconLog.logFile.close()
# Save network parameters
saveParameters("ceiling_evo", Xcon.netStruct, Ycon.netStruct,
Zcon.netStruct)
# Plotting the results
# logData.plotFigure()
# xconLog.plotControlData()
# yconLog.plotControlData()
# zconLog.plotControlData()
text = colored('Mission Complete!!',
'green',
attrs=['reverse', 'blink'])
print(text)
break
t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + ysqr) X = mt.degrees(mt.atan2(t0, t1)) t2 = +2.0 * (w * y - z * x) t2 = +1.0 if t2 > +1.0 else t2 t2 = -1.0 if t2 < -1.0 else t2 Y = mt.degrees(mt.asin(t2)) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (ysqr + z * z) Z = mt.degrees(mt.atan2(t3, t4)) return X, Y, Z local_velocity = TwistStamped() def lv_cb(data): global local_velocity local_velocity = data local_position = PoseStamped() def lp_cb(data): global local_position local_position = data def state_cb(data): global current_state current_state = data
def add_sentence(self, nmea_string, frame_id, timestamp=None):
"""Public method to provide a new NMEA sentence to the driver.
Args:
nmea_string (str): NMEA sentence in string form.
frame_id (str): TF frame ID of the GPS receiver.
timestamp(rospy.Time, optional): Time the sentence was received.
If timestamp is not specified, the current time is used.
Returns:
bool: True if the NMEA string is successfully processed, False if there is an error.
"""
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
if not self.use_GNSS_time:
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with
# epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (2 * hdop *
self.alt_std_dev)**2 # FIXME
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as
# well.
if self.valid_fix:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * math.sin(
data['true_course'])
current_vel.twist.linear.y = data['speed'] * math.cos(
data['true_course'])
self.vel_pub.publish(current_vel)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
if self.use_GNSS_time:
if math.isnan(data['utc_time'][0]):
rospy.logwarn("Time in the NMEA sentence is NOT valid")
return False
current_fix.header.stamp = rospy.Time(data['utc_time'][0],
data['utc_time'][1])
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time):
current_time_ref.time_ref = rospy.Time(
data['utc_time'][0], data['utc_time'][1])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide
# it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
else:
return False
def twistPub():
rospy.init_node("input_series_twist", anonymous=True)
pub = rospy.Publisher("twist_controller/command_twist_stamped",
TwistStamped,
queue_size=1)
rospy.sleep(1.0)
twist_msg = TwistStamped()
twist_msg.header.stamp = rospy.Time.now()
twist_msg.header.frame_id = "arm_left_base_link"
twist_msg.twist.linear.x = 0
twist_msg.twist.linear.y = 0
twist_msg.twist.linear.z = 0
twist_msg.twist.angular.x = 0
twist_msg.twist.angular.y = 0
twist_msg.twist.angular.z = 0
rate = 50
r = rospy.Rate(rate)
for i in range(0, 5 * rate, 1):
twist_msg.header.stamp = rospy.Time.now()
twist_msg.header.frame_id = "arm_left_base_link"
twist_msg.twist.linear.x = 0
twist_msg.twist.linear.y = -0.02
twist_msg.twist.linear.z = 0
twist_msg.twist.angular.x = 0
twist_msg.twist.angular.y = 0
twist_msg.twist.angular.z = 0
pub.publish(twist_msg)
r.sleep()
#pause
for i in range(0, 1 * rate, 1):
twist_msg.header.stamp = rospy.Time.now()
twist_msg.header.frame_id = "arm_left_base_link"
twist_msg.twist.linear.x = 0
twist_msg.twist.linear.y = 0
twist_msg.twist.linear.z = 0
twist_msg.twist.angular.x = 0
twist_msg.twist.angular.y = 0
twist_msg.twist.angular.z = 0
pub.publish(twist_msg)
r.sleep()
for i in range(0, 5 * rate, 1):
twist_msg.header.stamp = rospy.Time.now()
twist_msg.header.frame_id = "arm_left_base_link"
twist_msg.twist.linear.x = 0
twist_msg.twist.linear.y = 0.02
twist_msg.twist.linear.z = 0
twist_msg.twist.angular.x = 0
twist_msg.twist.angular.y = 0
twist_msg.twist.angular.z = 0
pub.publish(twist_msg)
r.sleep()
#pause
for i in range(0, 1 * rate, 1):
twist_msg.header.stamp = rospy.Time.now()
twist_msg.header.frame_id = "arm_left_base_link"
twist_msg.twist.linear.x = 0
twist_msg.twist.linear.y = 0
twist_msg.twist.linear.z = 0
twist_msg.twist.angular.x = 0
twist_msg.twist.angular.y = 0
twist_msg.twist.angular.z = 0
pub.publish(twist_msg)
r.sleep()
print "done"
def copy_vel(self, vel):
copied_vel = TwistStamped()
copied_vel.header = vel.header
return copied_vel
def callback(data): data_new = TwistStamped() data_new.header.stamp = rospy.Time.now() data_new.twist = data pub2.publish(data_new)
import math from geometry_msgs.msg import (PoseWithCovariance, Pose, Twist, TwistStamped) navDataRotZ = 0 navDataRotZ360 = 0 actionState = 0 latchStartTime = rospy.Duration(5.0) externalEstimatedPose = PoseWithCovariance() lastSavedWayHomePoint = Pose() targetInDrone = Pose() land_msg = Empty() takeoff_msg = Empty() reset_msg = Empty() messageTwist = Twist() messageTwistStamped = TwistStamped() targetInDrone = Pose() targetInMap = Pose() lastSavedWaypoint = Pose() realPose = PoseStamped( ) #TODO fix this error, only happens once, but still works. AttributeError: 'PoseStamped' object has no attribute 'position' droneWaypointsFromXML = DroneWaypoint() latched = False flying = False targetInMap.position.x = 0 targetInMap.position.y = 0 targetInMap.position.z = 0 targetInMap.orientation.x = 0 targetInMap.orientation.y = 0 targetInMap.orientation.z = 0
def onTwistStamped(self,msg): out_msg = TwistStamped() out_msg.header = rospy.time.now() out_msg.twist = msg self.twist_pub.publish(out_msg)
def spin_once(self):
def quat_from_orient(orient):
'''Build a quaternion from orientation data.'''
try:
w, x, y, z = orient['quaternion']
return (x, y, z, w)
except KeyError:
pass
try:
return quaternion_from_euler(pi*orient['roll']/180.,
pi*orient['pitch']/180, pi*orient['yaw']/180.)
except KeyError:
pass
try:
m = identity_matrix()
m[:3,:3] = orient['matrix']
return quaternion_from_matrix(m)
except KeyError:
pass
# get data
data = self.mt.read_measurement()
# common header
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
# get data (None if not present)
temp = data.get('Temp') # float
raw_data = data.get('RAW')
imu_data = data.get('Calib')
orient_data = data.get('Orient')
velocity_data = data.get('Velocity')
position_data = data.get('Position')
rawgps_data = data.get('RAWGPS')
status = data.get('Status') # int
sample = data.get('Sample')
# TODO by @demmeln: use sample counter for timestamp
# correction. Watch out for counter wrap.
# create messages and default values
imu_msg = Imu()
imu_msg.orientation_covariance = (-1., )*9
imu_msg.angular_velocity_covariance = (-1., )*9
imu_msg.linear_acceleration_covariance = (-1., )*9
pub_imu = False
gps_msg = NavSatFix()
xgps_msg = GPSFix()
pub_gps = False
vel_msg = TwistStamped()
pub_vel = False
mag_msg = Vector3Stamped()
pub_mag = False
temp_msg = Float32()
pub_temp = False
# fill information where it's due
# start by raw information that can be overriden
if raw_data: # TODO warn about data not calibrated
pub_imu = True
pub_vel = True
pub_mag = True
pub_temp = True
# acceleration
imu_msg.linear_acceleration.x = raw_data['accX']
imu_msg.linear_acceleration.y = raw_data['accY']
imu_msg.linear_acceleration.z = raw_data['accZ']
imu_msg.linear_acceleration_covariance = (0., )*9
# gyroscopes
imu_msg.angular_velocity.x = raw_data['gyrX']
imu_msg.angular_velocity.y = raw_data['gyrY']
imu_msg.angular_velocity.z = raw_data['gyrZ']
imu_msg.angular_velocity_covariance = (0., )*9
vel_msg.twist.angular.x = raw_data['gyrX']
vel_msg.twist.angular.y = raw_data['gyrY']
vel_msg.twist.angular.z = raw_data['gyrZ']
# magnetometer
mag_msg.vector.x = raw_data['magX']
mag_msg.vector.y = raw_data['magY']
mag_msg.vector.z = raw_data['magZ']
# temperature
# 2-complement decoding and 1/256 resolution
x = raw_data['temp']
if x&0x8000:
temp_msg.data = (x - 1<<16)/256.
else:
temp_msg.data = x/256.
if rawgps_data:
if rawgps_data['bGPS']<self.old_bGPS:
pub_gps = True
# LLA
xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
# NED vel # TODO?
# Accuracy
# 2 is there to go from std_dev to 95% interval
xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
self.old_bGPS = rawgps_data['bGPS']
if temp is not None:
pub_temp = True
temp_msg.data = temp
if imu_data:
try:
imu_msg.angular_velocity.x = imu_data['gyrX']
imu_msg.angular_velocity.y = imu_data['gyrY']
imu_msg.angular_velocity.z = imu_data['gyrZ']
imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
radians(0.025), 0., 0., 0., radians(0.025))
pub_imu = True
vel_msg.twist.angular.x = imu_data['gyrX']
vel_msg.twist.angular.y = imu_data['gyrY']
vel_msg.twist.angular.z = imu_data['gyrZ']
pub_vel = True
except KeyError:
pass
try:
imu_msg.linear_acceleration.x = imu_data['accX']
imu_msg.linear_acceleration.y = imu_data['accY']
imu_msg.linear_acceleration.z = imu_data['accZ']
imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
0.0004, 0., 0., 0., 0.0004)
pub_imu = True
except KeyError:
pass
try:
mag_msg.vector.x = imu_data['magX']
mag_msg.vector.y = imu_data['magY']
mag_msg.vector.z = imu_data['magZ']
pub_mag = True
except KeyError:
pass
if velocity_data:
pub_vel = True
vel_msg.twist.linear.x = velocity_data['Vel_X']
vel_msg.twist.linear.y = velocity_data['Vel_Y']
vel_msg.twist.linear.z = velocity_data['Vel_Z']
if orient_data:
pub_imu = True
orient_quat = quat_from_orient(orient_data)
imu_msg.orientation.x = orient_quat[0]
imu_msg.orientation.y = orient_quat[1]
imu_msg.orientation.z = orient_quat[2]
imu_msg.orientation.w = orient_quat[3]
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
if position_data:
pub_gps = True
xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
if status is not None:
if status & 0b0001:
self.stest_stat.level = DiagnosticStatus.OK
self.stest_stat.message = "Ok"
else:
self.stest_stat.level = DiagnosticStatus.ERROR
self.stest_stat.message = "Failed"
if status & 0b0010:
self.xkf_stat.level = DiagnosticStatus.OK
self.xkf_stat.message = "Valid"
else:
self.xkf_stat.level = DiagnosticStatus.WARN
self.xkf_stat.message = "Invalid"
if status & 0b0100:
self.gps_stat.level = DiagnosticStatus.OK
self.gps_stat.message = "Ok"
else:
self.gps_stat.level = DiagnosticStatus.WARN
self.gps_stat.message = "No fix"
self.diag_msg.header = h
self.diag_pub.publish(self.diag_msg)
if pub_gps:
if status & 0b0100:
gps_msg.status.status = NavSatStatus.STATUS_FIX
xgps_msg.status.status = GPSStatus.STATUS_FIX
gps_msg.status.service = NavSatStatus.SERVICE_GPS
xgps_msg.status.position_source = 0b01101001
xgps_msg.status.motion_source = 0b01101010
xgps_msg.status.orientation_source = 0b01101010
else:
gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
gps_msg.status.service = 0
xgps_msg.status.position_source = 0b01101000
xgps_msg.status.motion_source = 0b01101000
xgps_msg.status.orientation_source = 0b01101000
# publish available information
if pub_imu:
imu_msg.header = h
self.imu_pub.publish(imu_msg)
if pub_gps:
xgps_msg.header = gps_msg.header = h
self.gps_pub.publish(gps_msg)
self.xgps_pub.publish(xgps_msg)
if pub_vel:
vel_msg.header = h
self.vel_pub.publish(vel_msg)
if pub_mag:
mag_msg.header = h
self.mag_pub.publish(mag_msg)
if pub_temp:
self.temp_pub.publish(temp_msg)
def main():
if not len(sys.argv) == 2:
print "Usage: ./post_process.py <input.bag>"
sys.exit(1)
topics=["/bebop/states/ARDrone3/PilotingState/AttitudeChanged", "/bebop/cmd_vel", "/vicon/bebop_blue_en/bebop_blue_en", "/bebop/states/ARDrone3/PilotingState/SpeedChanged", "/vservo/cmd_vel"]
input_bag = sys.argv[1]
output_bag = os.path.splitext(os.path.basename(input_bag))[0] + "_processed.bag"
cmd_vel_stamped = TwistStamped()
cmd_vel_stamped.header.frame_id = "base_link"
print "Output: ", output_bag
i = 0
try:
i_bag = rosbag.Bag(input_bag)
o_bag = rosbag.Bag(output_bag, "w")
total_msgs = i_bag.get_message_count(topics)
for topic, msg, t in i_bag.read_messages(topics):
o_bag.write(topic, msg, t)
if topic == "/bebop/cmd_vel":
cmd_vel_stamped.header.stamp = t
cmd_vel_stamped.twist = msg
o_bag.write("/bebop/cmd_vel_stamped", cmd_vel_stamped, t)
i += 1
progress(i, total_msgs, output_bag)
finally:
o_bag.close()
i_bag.close()
