def _msg(self, quaternion, x_dist, y_dist, linear_speed, angular_speed, now, use_pose_ekf=False):
# next, we will publish the odometry message over ROS
msg = Odometry()
msg.header.frame_id = "odom"
msg.header.stamp = now
msg.pose.pose = Pose(Point(x_dist, y_dist, 0.0), quaternion)
msg.child_frame_id = "base_link"
msg.twist.twist = Twist(Vector3(linear_speed, 0, 0), Vector3(0, 0, angular_speed))
if use_pose_ekf:
# robot_pose_ekf needs these covariances and we may need to adjust them.
# From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py
# However, this is not needed because we are not using robot_pose_ekf
# odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0,
# 0, 1e-3, 0, 0, 0, 0,
# 0, 0, 1e6, 0, 0, 0,
# 0, 0, 0, 1e6, 0, 0,
# 0, 0, 0, 0, 1e6, 0,
# 0, 0, 0, 0, 0, 1e3]
#
# odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0,
# 0, 1e-3, 0, 0, 0, 0,
# 0, 0, 1e6, 0, 0, 0,
# 0, 0, 0, 1e6, 0, 0,
# 0, 0, 0, 0, 1e6, 0,
# 0, 0, 0, 0, 0, 1e3]
pass
return msg
def tf_and_odom(self, zumo_msg):
"""Broadcast tf transform and publish odometry. It seems a little
redundant to do both, but both are needed if we want to use the ROS
navigation stack."""
trans = (zumo_msg.x, zumo_msg.y, 0)
rot = tf.transformations.quaternion_from_euler(0, 0, zumo_msg.theta)
self.broadcaster.sendTransform(trans, rot, \
rospy.Time.now(), \
"base_link", \
"odom")
odom = Odometry()
odom.header.frame_id = 'odom'
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = trans[0]
odom.pose.pose.position.y = trans[1]
odom.pose.pose.position.z = trans[2]
odom.pose.pose.orientation.x = rot[0]
odom.pose.pose.orientation.y = rot[1]
odom.pose.pose.orientation.z = rot[2]
odom.pose.pose.orientation.w = rot[3]
odom.child_frame_id = 'base_link'
odom.twist.twist = self.stored_twist
self.odomPublisher.publish(odom)
def getOdometry(odometry_line):
# Timestamp [seconds.microseconds]
# Rolling Counter [signed 16bit integer]
# TicksRight [ticks]
# TicksLeft [ticks]
# X [m]*
# Y [m]*
# Theta [rad]*
# => 1235603339.32339, 4103, 2464, 2559, 1.063, 0.008, -0.028
str_x, str_y, str_th = odometry_line.split(', ')[4:7]
current_time = rospy.Time.now()
th = float(str_th)
x = float(str_x)
y = float(str_y)
z = 0.0
roll = 0
pitch = 0
yaw = th
qx, qy, qz, qw = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
# BUILDING ODOMETRY
msg = Odometry()
msg.header.stamp = current_time
msg.header.frame_id = 'odom'
msg.child_frame_id = 'base_link'
msg.pose.pose.position = Point(x, y, z)
msg.pose.pose.orientation = Quaternion(qx, qy, qz, qw)
return msg
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = 'base_footprint'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth):
quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta)
current_time = rospy.Time.now()
br = tf.TransformBroadcaster()
br.sendTransform((cur_x, cur_y, 0),
tf.transformations.quaternion_from_euler(0, 0, cur_theta),
current_time,
"base_footprint",
"odom")
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = cur_x
odom.pose.pose.position.y = cur_y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = Quaternion(*quat)
odom.pose.covariance[0] = 0.01
odom.pose.covariance[7] = 0.01
odom.pose.covariance[14] = 99999
odom.pose.covariance[21] = 99999
odom.pose.covariance[28] = 99999
odom.pose.covariance[35] = 0.01
odom.child_frame_id = 'base_footprint'
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
odom.twist.covariance = odom.pose.covariance
self.odom_pub.publish(odom)
def publish_odom(event):
global motor_controller, od
pos = motor_controller.position
covariance = Config.get_covariance_matrix()
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
if rospy.get_time() - pos.position_time > 50.0/1000:
odom.header.stamp = rospy.Time.now()
else:
odom.header.stamp = rospy.Time.from_sec(pos.position_time)
odom.pose.pose.position.x = pos.x
odom.pose.pose.position.y = pos.y
odom.pose.pose.orientation.z = math.sin(pos.yaw / 2)
odom.pose.pose.orientation.w = math.cos(pos.yaw / 2)
odom.pose.covariance = covariance
odom.twist.twist.linear.x = pos.linear_vel
odom.twist.twist.angular.z = pos.angular_vel
odom.twist.covariance = covariance
odom_publisher.publish(odom)
def publish_odometry(self, odom_combined):
"""
use current pose, along with delta from last pose to publish
the current Odometry on the /odom topic
"""
if not self.last_time:
# set up initial times and pose
rospy.loginfo("Setting up initial position")
self.last_time, self.last_x, y, self.last_theta = self.current_pose(odom_combined)
return
# publish to the /odom topic
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "/base_link"
odom.pose = odom_combined.pose
current_time, x, y, theta = self.current_pose(odom_combined)
dt = current_time - self.last_time
dt = dt.to_sec()
d_x = x - self.last_x
d_theta = theta - self.last_theta
odom.twist.twist = Twist(Vector3(d_x/dt, 0, 0), Vector3(0, 0, d_theta/dt))
self.odom_publisher.publish(odom)
self.last_time, self.last_x, self.last_theta = current_time, x, theta
def Publish_Odom_Tf(self): #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) quaternion = Quaternion() quaternion.x = 0 quaternion.y = 0 quaternion.z = math.sin(self.Heading / 2.0) quaternion.w = math.cos(self.Heading / 2.0) rosNow = rospy.Time.now() self._tf_broad_caster.sendTransform( (self.X_Pos, self.Y_Pos, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rosNow, "base_footprint", "odom" ) # next, we'll publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = rosNow odometry.pose.pose.position.x = self.X_Pos odometry.pose.pose.position.y = self.Y_Pos odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = self.Velocity odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = self.Omega self._odom_publisher.publish(odometry)
def odom_transform_2d(self, data, new_frame, trans, rot):
# NOTES: only in 2d rotation
# also, it removes covariance, etc information
odom_new = Odometry()
odom_new.header = data.header
odom_new.header.frame_id = new_frame
odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0]
odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1]
odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2]
# rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot))))
q = data.pose.pose.orientation
q_tuple = (q.x, q.y, q.z, q.w,)
# Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion
odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot))
heading_change = quaternion_to_heading(rot)
odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change)
odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change)
odom_new.twist.twist.linear.z = 0
odom_new.twist.twist.angular.x = 0
odom_new.twist.twist.angular.y = 0
odom_new.twist.twist.angular.z = data.twist.twist.angular.z
return odom_new
def poll(self):
(x, y, theta) = self.arduino.arbot_read_odometry()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
# Create the odometry transform frame broadcaster.
now = rospy.Time.now()
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
now,
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.forwardSpeed
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.angularSpeed
self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)
self.odomPub.publish(odom)
def Talk(self, time):
#print self.orientation.get_euler()['yaw'], self.position.yaw, self.sensors['gps'].position.yaw
msgs = Odometry( )
msgs.header.stamp = rospy.Time.now()
msgs.header.frame_id = "/nav"
msgs.child_frame_id = "/drone_local"
msgs.pose.pose.position.x = self.position.x
msgs.pose.pose.position.y = self.position.y
msgs.pose.pose.position.z = self.position.z
msgs.pose.pose.orientation.x = self.orientation.x
msgs.pose.pose.orientation.y = self.orientation.y
msgs.pose.pose.orientation.z = self.orientation.z
msgs.pose.pose.orientation.w = self.orientation.w
msgs.twist.twist.linear.x = self.velocity.x
msgs.twist.twist.linear.y = self.velocity.y
msgs.twist.twist.linear.z = self.velocity.z
#rospy.loginfo(msgs)
self.publisher['state'].publish(msgs)
self.tf_broadcaster['drone_local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) ,
self.orientation.get_quaternion( ),
msgs.header.stamp,
msgs.child_frame_id,
msgs.header.frame_id)
def publish_odom(self, robot):
# Transform etc
x = robot.x
y = robot.y
th = robot.th
br = tf.TransformBroadcaster()
quat = tf.transformations.quaternion_from_euler(0,0,th)
odom_trans = TransformStamped()
odom_trans.header.stamp = rospy.Time.now()
odom_trans.header.frame_id = "/map"
odom_trans.child_frame_id = "/base_link"
odom_trans.transform.translation.x = x
odom_trans.transform.translation.y = y
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = quat;
#tf.TransformBroadcaster().SendTransform(R,t,time,child,parent)
br.sendTransform((x,y,0.0), quat, odom_trans.header.stamp, odom_trans.child_frame_id, odom_trans.header.frame_id)
# Publish Odom msg
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "/map"
odom_msg.child_frame_id = "/base_link"
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
odom_msg.pose.pose.position.z = 0.0
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
# publish
self.odomPub.publish(odom_msg)
def callback(msg):
pub = rospy.Publisher("odom_covar", Odometry)
corrected = Odometry()
corrected.header.seq = msg.header.seq
corrected.header.stamp = rospy.Time.now()
corrected.header.frame_id = msg.header.frame_id
corrected.child_frame_id = "base_footprint"
corrected.pose.pose = msg.pose.pose
corrected.pose.covariance = [.01, 0, 0, 0, 0, 0,
0, .01, 0, 0, 0, 0,
0, 0, .01, 0, 0, 0,
0, 0, 0, .01, 0, 0,
0, 0, 0, 0, .01, 0,
0, 0, 0, 0, 0, .01]
corrected.twist.twist = msg.twist.twist
corrected.twist.covariance = [.01, 0, 0, 0, 0, 0,
0, .01, 0, 0, 0, 0,
0, 0, .01, 0, 0, 0,
0, 0, 0, .01, 0, 0,
0, 0, 0, 0, .01, 0,
0, 0, 0, 0, 0, .01]
pub.publish(corrected)
def update_vel():
"Main loop"""
global g_last_loop_time, g_vel_x, g_vel_y, g_vel_dt, x_pos, y_pos
#while not rospy.is_shutdown():
current_time = rospy.Time.now()
linear_velocity_x = g_vel_x
linear_velocity_y = g_vel_y
# Calculate current position of the robot
x_pos += linear_velocity_x * g_vel_dt
y_pos += linear_velocity_y * g_vel_dt
# All odometry is 6DOF, so need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x_pos, y_pos, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_footprint"
odom.twist.twist = Twist(
Vector3(linear_velocity_x, linear_velocity_y, 0), Vector3(0, 0, 0))
# publish the message
odom_pub.publish(odom)
g_last_loop_time = current_time
def handle_harlie_pose(msg, push_casters): pose = changePoseCoordFrame(msg) current_time = rospy.Time.now() theta = pose.theta + pi if push_casters else pose.theta quaternion = tf.transformations.quaternion_about_axis(theta, (0,0,1)) tf_br.sendTransform(translation = (pose.x, pose.y, 0), rotation = tuple(quaternion), time = current_time, child = 'base_link', parent = 'odom') odom_msg = Odometry() odom_msg.header.stamp = current_time odom_msg.header.frame_id = 'odom' odom_msg.pose.pose.position.x = pose.x odom_msg.pose.pose.position.y = pose.y odom_msg.pose.pose.position.z = 0.0 odom_msg.pose.pose.orientation = Quaternion(*quaternion) odom_msg.pose.covariance[0] = pose.x_var odom_msg.pose.covariance[7] = pose.y_var odom_msg.pose.covariance[35] = pose.theta_var odom_msg.child_frame_id = 'base_link' odom_msg.twist.twist.linear.x = pose.vel odom_msg.twist.twist.angular.z = pose.omega odom_msg.twist.covariance[0] = pose.vel_var odom_msg.twist.covariance[35] = pose.omega_var odom_pub.publish(odom_msg)
def main():
rospy.init_node("pose2d_to_odometry")
own_num = rospy.get_param('~own_num', 0)
global odom, head
head = Header()
head.stamp = rospy.Time.now()
head.frame_id = "robot_{}/odom_combined".format(own_num)
odom = Odometry()
odom.child_frame_id = "robot_{}/base_footprint".format(own_num)
o = 10**-9 # zero
odom.pose.covariance = [1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, o, 0, 0, 0, # z doesn't change
0, 0, 0, o, 0, 0, # constant roll
0, 0, 0, 0, o, 0, # constant pitch
0, 0, 0, 0, 0, o] # guess - .3 radians rotation cov
odom.twist.covariance = [100, 0, 0, 0, 0, 0,
0, 100, 0, 0, 0, 0,
0, 0, 100, 0, 0, 0, # large covariances - no data
0, 0, 0, 100, 0, 0,
0, 0, 0, 0, 100, 0,
0, 0, 0, 0, 0, 100]
global odom_pub
odom_topic = rospy.get_param('~odometry_publish_topic', 'vo')
pose2D_topic = rospy.get_param('~pose2D_topic', 'pose2D')
odom_pub = rospy.Publisher(odom_topic, Odometry)
rospy.Subscriber(pose2D_topic, Pose2D, on_pose)
rospy.spin()
def onNavSts(self,data):
q = tf.transformations.quaternion_from_euler(math.pi, 0, math.pi/2)
self.broadcaster.sendTransform((0,0,0), q.conjugate(), rospy.Time.now(), "uwsim_frame", "local");
try:
(trans,rot) = self.listener.lookupTransform("uwsim_frame", self.base_frame, rospy.Time(0))
odom = Odometry();
odom.twist.twist.linear.x = data.body_velocity.x;
odom.twist.twist.linear.y = data.body_velocity.y;
odom.twist.twist.linear.z = data.body_velocity.z;
odom.twist.twist.angular.x = data.orientation_rate.roll;
odom.twist.twist.angular.y = data.orientation_rate.pitch;
odom.twist.twist.angular.z = data.orientation_rate.yaw;
odom.child_frame_id = self.base_frame;
odom.pose.pose.orientation.x = rot[0];
odom.pose.pose.orientation.y = rot[1];
odom.pose.pose.orientation.z = rot[2];
odom.pose.pose.orientation.w = rot[3];
odom.pose.pose.position.x = trans[0];
odom.pose.pose.position.y = trans[1];
odom.pose.pose.position.z = trans[2];
odom.header.stamp = rospy.Time.now();
odom.header.frame_id = "local";
self.pub.publish(odom);
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
None
def __update_odometry(self, linear_offset, angular_offset, tf_delay): self.__heading = (self.__heading + angular_offset) % 360 q = tf.transformations.quaternion_from_euler(0, 0, math.radians(self.__heading)) self.__pose.position.x += math.cos(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.y += math.sin(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.z = 0.33 self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) now = rospy.Time.now() + rospy.Duration(tf_delay) self.__tfb.sendTransform( (self.__pose.position.x, self.__pose.position.y, self.__pose.position.z), q, now, 'base_link', 'odom') o = Odometry() o.header.stamp = now o.header.frame_id = 'odom' o.child_frame_id = 'base_link' o.pose = PoseWithCovariance(self.__pose, None) o.twist = TwistWithCovariance() o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x o.twist.twist.angular.z = math.radians(self.__rotation_per_second) * self.__twist.angular.z
def publish_Tfs():
global tf_rot,tf_trans,tf_tda,tf_rda,tf_tbc,tf_rbc,scale
br = tf.TransformBroadcaster()
rate = rospy.Rate(50.0)
while not rospy.is_shutdown():
rate.sleep()
camOdom = Odometry()
camOdom.header.stamp = rospy.Time.now()
camOdom.header.frame_id = "/odom_combined"
camOdom.pose.pose.position.x =0
camOdom.pose.pose.position.y =0
camOdom.pose.pose.position.z =0. #Tbc[2]
M=np.identity(4)
q=tf.transformations.quaternion_from_matrix(M)
camOdom.pose.pose.orientation.x = q[0]
camOdom.pose.pose.orientation.y = q[1]
camOdom.pose.pose.orientation.z = q[2]
camOdom.pose.pose.orientation.w = q[3]
camOdom.child_frame_id = "/base_footprint"
camOdom.twist.twist.linear.x = 0
camOdom.twist.twist.linear.y = 0
camOdom.twist.twist.angular.z = 0
T=np.array([0.,0.,0.])
M=np.identity(4)
q=tf.transformations.quaternion_from_matrix(M)
br.sendTransform(T,q,
rospy.Time.now(),
"/base_footprint",
"/odom_combined")
camOdomPub.publish(camOdom)
def talker(message):
corrected = Odometry()
corrected.header.seq = message.header.seq
corrected.header.stamp = rospy.Time.now()
corrected.header.frame_id = message.header.frame_id #base_footprint #message.header.frame_id
corrected.child_frame_id = message.child_frame_id #"base_footprint"
corrected.pose.pose = message.pose.pose
corrected.pose.covariance = [0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.001, 0, 0,
0, 0, 0, 0, 0.001, 0,
0, 0, 0, 0, 0, .5]
corrected.twist.twist = message.twist.twist
corrected.twist.covariance = [0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.001, 0, 0,
0, 0, 0, 0, 0.001, 0,
0, 0, 0, 0, 0, .5]
pub = rospy.Publisher('odom_w_cov', Odometry)
pub.publish(corrected)
def pubOdometry(self, event):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = str(self.frame_id)
odom.child_frame_id = "real_girona500"
odom.pose.pose.position.x = self.p[0]
odom.pose.pose.position.y = self.p[1]
odom.pose.pose.position.z = self.p[2]
orientation = tf.transformations.quaternion_from_euler(self.p[3], self.p[4], self.p[5], 'sxyz')
odom.pose.pose.orientation.x = orientation[0]
odom.pose.pose.orientation.y = orientation[1]
odom.pose.pose.orientation.z = orientation[2]
odom.pose.pose.orientation.w = orientation[3]
#Haig de comentar les velocitats sino l'UWSim no va
odom.twist.twist.linear.x = 0.0 #v_[0]
odom.twist.twist.linear.y = 0.0 #v_[1]
odom.twist.twist.linear.z = 0.0 #v_[2]
odom.twist.twist.angular.x = 0.0 #v_[3]
odom.twist.twist.angular.y = 0.0 #v_[4]
odom.twist.twist.angular.z = 0.0 #v_[5]
self.pub_odom.publish(odom)
# Broadcast transform
br = tf.TransformBroadcaster()
br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation,
odom.header.stamp, odom.header.frame_id, "world")
def post_odometry(self, component_instance):
""" Publish the data of the Pose as a Odometry message for fake localization
"""
parent_name = component_instance.robot_parent.blender_obj.name
component_name = component_instance.blender_obj.name
frame_id = self._properties[component_name]['frame_id']
child_frame_id = self._properties[component_name]['child_frame_id']
odometry = Odometry()
odometry.header.stamp = rospy.Time.now()
odometry.header.frame_id = frame_id
odometry.child_frame_id = child_frame_id
odometry.pose.pose.position.x = component_instance.local_data['x']
odometry.pose.pose.position.y = component_instance.local_data['y']
odometry.pose.pose.position.z = component_instance.local_data['z']
euler = mathutils.Euler((component_instance.local_data['roll'],
component_instance.local_data['pitch'],
component_instance.local_data['yaw']))
quaternion = euler.to_quaternion()
odometry.pose.pose.orientation = quaternion
for topic in self._topics:
# publish the message on the correct topic
if str(topic.name) == str("/" + parent_name + "/" + component_name):
topic.publish(odometry)
def Talk(self):
msgs = Odometry()
msgs.header.stamp = rospy.Time.now()
msgs.header.frame_id = '/nav'
msgs.child_frame_id = self.name
msgs.pose.pose.position.x = self.position.x
msgs.pose.pose.position.y = self.position.y
msgs.pose.pose.position.z = self.position.z
msgs.pose.pose.orientation.x = self.orientation.x
msgs.pose.pose.orientation.y = self.orientation.y
msgs.pose.pose.orientation.z = self.orientation.z
msgs.pose.pose.orientation.w = self.orientation.w
msgs.twist.twist.linear.x = self.velocity.x
msgs.twist.twist.linear.y = self.velocity.y
msgs.twist.twist.linear.z = self.velocity.z
#rospy.loginfo(msgs)
self.publisher['state'].publish(msgs)
self.tf_broadcaster['local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) ,
(msgs.pose.pose.orientation.x, msgs.pose.pose.orientation.y, msgs.pose.pose.orientation.z, msgs.pose.pose.orientation.w),
msgs.header.stamp,
msgs.child_frame_id,
msgs.header.frame_id)
def handle_imu(self, imu_data):
if not self.imu_recv:
rospy.loginfo('IMU data received')
self.imu_recv = True
if self.pub.get_num_connections() != 0:
msg = Odometry()
#msg.header.frame_id = imu_data.header.frame_id
msg.header.frame_id = 'imu_frame'
msg.header.stamp = imu_data.header.stamp
msg.child_frame_id = 'imu_base_link'
msg.pose.pose.position.x = 0.0 # here, we can maybe try to integrate accelerations...
msg.pose.pose.position.y = 0.0
msg.pose.pose.position.z = 0.0
msg.pose.pose.orientation = imu_data.orientation # should we transform this orientation into base_footprint frame????
msg.pose.covariance = [99999, 0, 0, 0, 0, 0, # large covariance on x, y, z
0, 99999, 0, 0, 0, 0,
0, 0, 99999, 0, 0, 0,
0, 0, 0, 1e-06, 0, 0, # we trust in rpy
0, 0, 0, 0, 1e-06, 0,
0, 0, 0, 0, 0, 1e-06]
# does robot_pose_ekf care about twist?
self.pub.publish(msg)
def callback(data):
print data
odom = Odometry()
odom.pose = data.pose
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "/odom"
global pub
pub.publish(odom)
def listener():
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/wiimote1/imu", Imu, wiimote1Callback)
rospy.Subscriber("/wiimote2/imu", Imu, wiimote2Callback)
odomPub = rospy.Publisher("/base_controller/odom", Odometry)
odom = Odometry()
while 1:
br = tf.TransformBroadcaster()
time.sleep(.01)
robot.updateState()
if robot.readyToPublish:
quaternion = tf.transformations.quaternion_from_euler(0, 0, robot.theta)
#tf msg
#br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/odom", "/map")
br.sendTransform((robot.x, robot.y, 0), tf.transformations.quaternion_from_euler(0, 0, robot.theta),rospy.Time.now(),"/base_link", "/odom")
br.sendTransform((0, 0, .7), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/base_laser_link", "/base_link")
br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/camera1_link", "/base_laser_link")
br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 3.14),rospy.Time.now(),"/camera2_link", "/base_laser_link")
robot.readyToPublish = False
#odometry msg
odom.header.stamp = robot.lastTime
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = robot.x
odom.pose.pose.position.y = robot.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation.x = quaternion[0]
odom.pose.pose.orientation.y = quaternion[1]
odom.pose.pose.orientation.z = quaternion[2]
odom.pose.pose.orientation.w = quaternion[3]
odom.twist.twist.linear.x = robot.xV
odom.twist.twist.linear.y = robot.yV
# the x angular velocity corresponds to left wheel and y to right wheel
odom.twist.twist.angular.x = robot.leftV
odom.twist.twist.angular.y = robot.rightV
odom.twist.twist.angular.z = robot.thetaV
# place holder covariance matrix
P = [.0001, 0, 0, 0, 0, 0,
0, .0001, 0, 0, 0, 0,
0, 0, .0001, 0, 0, 0,
0, 0, 0, .0001, 0, 0,
0, 0, 0, 0, .0001, 0,
0, 0, 0, 0, 0, .0001]
odom.pose.covariance = P
odomPub.publish(odom)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def inspvax_handler(self, inspvax):
# Convert the latlong to x,y coordinates and publish an Odometry
try:
utm_pos = geodesy.utm.fromLatLong(inspvax.latitude, inspvax.longitude)
except ValueError:
# Probably coordinates out of range for UTM conversion.
return
if not self.init and self.zero_start:
self.origin.x = utm_pos.easting
self.origin.y = utm_pos.northing
self.origin.z = inspvax.altitude
self.pub_origin.publish(position=self.origin)
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = utm_pos.easting - self.origin.x
odom.pose.pose.position.y = utm_pos.northing - self.origin.y
odom.pose.pose.position.z = inspvax.altitude - self.origin.z
# Orientation
# Save this on an instance variable, so that it can be published
# with the IMU message as well.
self.orientation = tf.transformations.quaternion_from_euler(
radians(inspvax.pitch), radians(inspvax.roll), radians(90 - inspvax.azimuth)
)
print inspvax.azimuth
odom.pose.pose.orientation = Quaternion(*self.orientation)
odom.pose.covariance[21] = self.orientation_covariance[0] = pow(2, inspvax.pitch_std)
odom.pose.covariance[28] = self.orientation_covariance[4] = pow(2, inspvax.roll_std)
odom.pose.covariance[35] = self.orientation_covariance[8] = pow(2, inspvax.azimuth_std)
# Twist is relative to vehicle frame
odom.twist.twist.linear.x = inspvax.east_velocity
odom.twist.twist.linear.y = inspvax.north_velocity
odom.twist.twist.linear.z = inspvax.up_velocity
TWIST_COVAR[0] = pow(2, inspvax.east_velocity_std)
TWIST_COVAR[7] = pow(2, inspvax.north_velocity_std)
TWIST_COVAR[14] = pow(2, inspvax.up_velocity_std)
# odom.twist.twist.angular = imu.angular_velocity
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
# Odometry transform (if required)
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z),
self.orientation,
odom.header.stamp,
odom.child_frame_id,
odom.header.frame_id,
)
# Mark that we've received our first fix, and set origin if necessary.
self.init = True
def publish(self, data):
q = Quaternion()
q.x = 0
q.y = 0
q.z = data[6]
q.w = data[7]
odomMsg = Odometry()
odomMsg.header.frame_id = self._odom
odomMsg.header.stamp = rospy.get_rostime()
odomMsg.child_frame_id = self._baseLink
odomMsg.pose.pose.position.x = data[0]
odomMsg.pose.pose.position.y = data[1]
odomMsg.pose.pose.position.z = 0
odomMsg.pose.pose.orientation = q
if self._firstTimePub:
self._prevOdom = odomMsg
self._firstTimePub = False
return
velocity = Twist()
deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec()
yaw, pitch, roll = euler_from_quaternion(
[odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y,
odomMsg.pose.pose.orientation.z])
prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion(
[self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x,
self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z])
if deltaTime > 0:
velocity.linear.x = (data[8] / deltaTime)
deltaYaw = yaw - prevYaw
# rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw))
if deltaYaw > math.pi: deltaYaw -= 2 * math.pi
elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi
if deltaTime > 0:
velocity.angular.z = -(deltaYaw / deltaTime)
# rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z))
odomMsg.twist.twist = velocity
self._prevOdom = odomMsg
traMsg = TransformStamped()
traMsg.header.frame_id = self._odom
traMsg.header.stamp = rospy.get_rostime()
traMsg.child_frame_id = self._baseLink
traMsg.transform.translation.x = data[0]
traMsg.transform.translation.y = data[1]
traMsg.transform.translation.z = 0
traMsg.transform.rotation = q
self._pub.publish(odomMsg)
self._broadCase.sendTransformMessage(traMsg)
def callback(pose):
# cov_east = navsatfix.position_covariance[0]
# cov_north = navsatfix.position_covariance[4]
# if cov_east>cov_thresh or cov_north>cov_thresh:
# return
odom = Odometry()
odom.pose.pose = pose.pose
odom.header = pose.header
pub.publish(odom)
def transition(self, state, twist, dt):
desired = state[1]
current = state[0]
# do something
n = Odometry()
n.header = current.header
n.twist.twist = self.update_twist(current.twist.twist, twist)
n.pose.pose = self.update_pose(current.pose.pose, current.twist.twist, n.twist.twist, dt)
return (n, desired, )
def trajectory_in_camera_fov(self, data):
"""generates trajecory in camera workspace"""
r = 9.5
self.end_point = np.array([
r * np.sin((self.RHP_time - 5) / 10), -r * np.cos(
(self.RHP_time - 5) / 10), 2
])
#self.end_point = self.end_point + np.array([self.target_velocity*0.05, 0, 0])
start = time.time()
odom_msg = Odometry()
odom_msg.pose.pose.position.x = self.end_point[0]
odom_msg.pose.pose.position.y = self.end_point[1]
odom_msg.pose.pose.position.z = self.end_point[2]
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = 'world'
self.target_odom.publish(odom_msg)
if self.traj_gen_counter == 0 or self.RHP_time >= self.tracking_in_global_time - 0.1:
t1 = time.time() - start
#this bit of code is to be commented out to generate the cloud seperately
start_point = np.zeros((0, 3))
if self.traj_gen_counter != 0:
start_point = np.concatenate((start_point, self.p_eoe), 0)
else:
start_point = np.concatenate(
(start_point, self.Pglobal[np.newaxis]), 0)
#if self.traj_gen_counter != 0:
# #start_point = np.concatenate((start_point, self.p_eoe), 0)
# self.Pglobal = np.dot(self.Rglobal.T, self.Rcg_vibase[0:3, 3:4]) + np.dot(self.Rcg_vibase[0:3, 0:3], self.Pglobal[np.newaxis].T)
# start_point = np.concatenate((start_point, self.Pglobal.T), 0) # was self.p_eoe
#else:
# # need to get a point corresponds to self.Pglobal at the apex of the grid
# # keep on adding newaxis and .T because you do not maintain uniformity
# self.Pglobal = -self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3], self.Pglobal[np.newaxis].T)
# start_point = np.concatenate((start_point, self.Pglobal.T), 0) # new axis was needed because you dont maintain uniformity
#start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0)
points_in_cone = self.generate_regular_pyramid_grid()
occupied_points = ros_numpy.numpify(data)
if len(occupied_points) != 0:
#print occupied_points
points_in_cone = self.generate_final_grid(
points_in_cone, occupied_points)
p1 = [
np.dot(self.Rcdoc_cdl[0:3, 0:3], x[np.newaxis].T)
for x in points_in_cone
]
p2 = [
self.Rcg_vibase[0:3, 3:4] +
np.dot(np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), x) for x in p1
]
points_in_cone = [
self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2
]
points_in_cone = [x for x in points_in_cone if not x[2] <= 0]
#kdt_points_in_cone = cKDTree(points_in_cone)
#if len(occupied_points) == 0:
#print start_point, len(points_in_cone), len(occupied_points)
#if self.traj_gen_counter != 0:
points_in_cone = np.concatenate((start_point, points_in_cone), 0)
# publish generated pointcloud
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
p = pc2.create_cloud_xyz32(header, points_in_cone)
self.pcl_pub.publish(p)
#this bit of code is to be commented out to generate the cloud seperately
#should be uncommented if cloud is genereted here
#points = ros_numpy.numpify(data)
#p = np.zeros((points.shape[0],3))
#p[:,0] = points['x']
#p[:,1] = points['y']
#p[:,2] = points['z']
#points_in_cone = p
kdt_points_in_cone = cKDTree(points_in_cone)
closest_to_end = kdt_points_in_cone.query(self.end_point, 1)
#closest_to_end_index = points_in_cone[closest_to_end[1]]
end_point_index = closest_to_end[1]
#print closest_to_end, points_in_cone[end_point_index]
planning_horizon = 4 # planning horizon
execution_horizon = 1 # execution horizon
dist_to_goal = np.linalg.norm(self.end_point - self.Pglobal)
ttt = self.RHP_time - 5.0
timefactor = 1 / (1 + np.exp(-1 * (ttt)))
distancefactor = erf(0.25 * (dist_to_goal - self.resolution))
smoothing_factor = timefactor * distancefactor
f0 = open('velocity.txt', 'a')
f0.write('%s, %s, %s, %s\n' %
(ttt, timefactor, distancefactor,
self.uav_velocity * smoothing_factor))
t2 = time.time() - start
#one dimension simplicial complex which is basically a graph
rips = gudhi.RipsComplex(points=points_in_cone,
max_edge_length=1.5 * self.resolution)
f = rips.create_simplex_tree(max_dimension=1)
# make graph
G = nx.Graph()
G.add_nodes_from(range(f.num_vertices()))
edge_list = [(simplex[0][0], simplex[0][1], {
'weight': simplex[1]
}) if len(simplex[0]) == 2 else None
for simplex in f.get_skeleton(1)]
edge_list = [k for k in edge_list if k is not None]
G.add_edges_from(edge_list)
try:
print 'i m here.................', points_in_cone[
0], points_in_cone[end_point_index]
shortest_path = nx.shortest_path(G,
source=0,
target=end_point_index,
weight='weight',
method='dijkstra')
path = np.array([points_in_cone[j] for j in shortest_path])
print 'length of the path is:', len(path) - 1
t3 = time.time() - start
if dist_to_goal < self.resolution:
self.reached_goal = True
if self.reached_goal == False:
if len(path) - 1 >= planning_horizon:
no_of_segments = planning_horizon
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments + 1]
elif execution_horizon <= len(path) - 1 < planning_horizon:
no_of_segments = len(path) - 1
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments + 1]
elif len(path) - 1 < execution_horizon:
no_of_segments = len(path) - 1
no_of_segments_to_track = no_of_segments
path = path[:no_of_segments + 1]
else:
if self.static_goal == True:
no_of_segments = len(path) - 1
no_of_segments_to_track = no_of_segments
path = path[:no_of_segments + 1]
else:
if len(path) - 1 >= planning_horizon:
no_of_segments = planning_horizon
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments + 1]
elif execution_horizon <= len(
path) - 1 < planning_horizon:
no_of_segments = len(path) - 1
no_of_segments_to_track = execution_horizon
path = path[:no_of_segments + 1]
elif len(path) - 1 < execution_horizon:
no_of_segments = len(path) - 1
no_of_segments_to_track = no_of_segments
path = path[:no_of_segments + 1]
#print '-----len of path is---', len(path)-1
path = zip(*path)
f0 = open('path.txt', 'a')
f0.write('%s\n' % (zip(*path)))
# now construct the minimum snap trajectory on the minimum path
waypoint_specified = True
waypoint_bc = False
t4 = time.time() - start
#erf1 = erf(1000*dist_to_goal)
#erf2 = erf(-1000*(dist_to_goal-self.initial_dist_to_goal))
velocity = self.uav_velocity * smoothing_factor
print '----------average velocity is--------', velocity
#f1 = open('velocity.txt', 'a')
#f1.write('%s, %s, %s, %s, %s, %s\n' %(self.initial_dist_to_goal, dist_to_goal, ttt, timefactor, distancefactor, velocity))
T, _p1, _p2, _p3 = Minimum_snap_trajetory(
self.replanning_started, velocity, path,
waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe,
self.j_eoe).construct_polynomial_trajectory()
t5 = time.time() - start
#self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)
N = 8
_pp1 = [_p1[i:i + N] for i in range(0, len(_p1), N)]
[i.reverse() for i in _pp1]
_pp2 = [_p2[i:i + N] for i in range(0, len(_p2), N)]
[i.reverse() for i in _pp2]
_pp3 = [_p3[i:i + N] for i in range(0, len(_p3), N)]
[i.reverse() for i in _pp3]
xx = np.poly1d(_pp1[no_of_segments_to_track - 1])
vx = np.polyder(xx, 1)
accx = np.polyder(xx, 2)
jx = np.polyder(xx, 3)
sx = np.polyder(xx, 4)
yy = np.poly1d(_pp2[no_of_segments_to_track - 1])
vy = np.polyder(yy, 1)
accy = np.polyder(yy, 2)
jy = np.polyder(yy, 3)
sy = np.polyder(yy, 4)
zz = np.poly1d(_pp3[no_of_segments_to_track - 1])
vz = np.polyder(zz, 1)
accz = np.polyder(zz, 2)
jz = np.polyder(zz, 3)
sz = np.polyder(zz, 4)
xdes = xx(T[no_of_segments_to_track])
ydes = yy(T[no_of_segments_to_track])
zdes = zz(T[no_of_segments_to_track])
vxdes = vx(T[no_of_segments_to_track])
vydes = vy(T[no_of_segments_to_track])
vzdes = vz(T[no_of_segments_to_track])
axdes = accx(T[no_of_segments_to_track])
aydes = accy(T[no_of_segments_to_track])
azdes = accz(T[no_of_segments_to_track])
jxdes = jx(T[no_of_segments_to_track])
jydes = jy(T[no_of_segments_to_track])
jzdes = jz(T[no_of_segments_to_track])
self.p_eoe = np.array([[xdes, ydes, zdes]])
# needed to transform the trajectory back to cog
#self.p_eoe = (-self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3], self.p_eoe.T)).T
self.v_eoe = np.array([[vxdes, vydes, vzdes]])
self.a_eoe = np.array([[axdes, aydes, azdes]])
self.j_eoe = np.array([[jxdes, jydes, jzdes]])
eoe_msg = Odometry()
eoe_msg.pose.pose.position.x = self.p_eoe[0][0]
eoe_msg.pose.pose.position.y = self.p_eoe[0][1]
eoe_msg.pose.pose.position.z = self.p_eoe[0][2]
eoe_msg.header.stamp = rospy.Time.now()
eoe_msg.header.frame_id = 'world'
self.eoe_odom.publish(eoe_msg)
#f1 = open('eoe_points.txt', 'a')
#f1.write('%s, %s\n' %(self.Pglobal, self.p_eoe))
#vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
vector = np.array([
self.end_point[0] - self.p_eoe[0][0],
self.end_point[1] - self.p_eoe[0][1],
self.end_point[2] - self.p_eoe[0][2]
])
#vector = np.array([1,0,0])#self.v_eoe
direction = vector / np.linalg.norm(vector)
t6 = time.time() - start
msg = PolynomialCoefficients()
header = std_msgs.msg.Header()
header.stamp = data.header.stamp #rospy.Time.now()
header.frame_id = 'world'
msg.header = header
msg.polynomial_order = N - 1
for i in range(len(_p1)):
msg.poly_x.append(_p1[i])
msg.poly_y.append(_p2[i])
msg.poly_z.append(_p3[i])
msg.number_of_segments = no_of_segments_to_track
msg.planning_start_time = data.header.stamp.to_sec()
for j in T[:no_of_segments_to_track + 1]:
msg.segment_end_times.append(j)
msg.desired_direction.x = direction[0]
msg.desired_direction.y = direction[1]
msg.desired_direction.z = direction[2]
#msg.desired_direction.x = direction[0][0]; msg.desired_direction.y = direction[0][1]; msg.desired_direction.z = direction[0][2]
msg.trajectory_mode = 'planning_in_camera_fov'
self.previous_coefficients = msg
self.traj_polycoeff.publish(msg)
self.republish_trajectory = True
f1 = open('total_time_taken.txt', 'a')
f1.write("%s, %s, %s, %s, %s, %s\n" % (t1, t2, t3, t4, t5, t6))
except:
print 'No path between start and end'
#vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
#direction = vector/np.linalg.norm(vector)
#self.previous_coefficients.desired_direction.x = direction[0]
#self.previous_coefficients.desired_direction.y = direction[1]
#self.previous_coefficients.desired_direction.z = direction[1]
self.traj_polycoeff.publish(self.previous_coefficients)
self.republish_trajectory = False
else: #elif self.RHP_time <= self.tracking_in_global_time - 0.1:
self.traj_polycoeff.publish(self.previous_coefficients)
self.republish_trajectory = False
time_taken = time.time() - start
#time.sleep(T[no_of_segments_to_track]-0.1-time_taken)
self.RHP_time = data.header.stamp.to_sec() - self.hover_start_time
#print 'total time taken to execute the callbacak is:', time_taken
self.traj_gen_counter += 1
def timerOdomCB(self, event):
# Serial read & publish
try:
myData = self.serial.readline(8).strip()
if len(myData) > 0:
WL = int(myData[1]) * 100 + int(myData[2]) * 10 + int(
myData[3])
WR = int(myData[5]) * 100 + int(myData[6]) * 10 + int(
myData[7])
WL = float(WL) / 100.0
WR = float(WR) / 100.0
if int(myData[0]) < 1:
WL = WL * (-1)
if int(myData[4]) < 1:
WR = WR * (-1)
rospy.loginfo(myData)
#print "serialRead success~"
# Twist
VL = WL * self.wheelRad * self.scale # V = omega * radius, unit: m/s
VR = WR * self.wheelRad * self.scale
Vyaw = (VR - VL) / self.wheelSep
Vx = (VR + VL) / 2.0
#print "Twist success~"
# Pose
self.current_time = rospy.Time.now()
dt = (self.current_time - self.previous_time).to_sec()
self.previous_time = self.current_time
self.pose_x = self.pose_x + Vx * math.cos(self.pose_yaw) * dt
self.pose_y = self.pose_y + Vx * math.sin(self.pose_yaw) * dt
self.pose_yaw = self.pose_yaw + Vyaw * dt
pose_quat = tf.transformations.quaternion_from_euler(
0, 0, self.pose_yaw)
#print "Pose success~"
# Publish Odometry
msg = Odometry()
msg.header.stamp = self.current_time
msg.header.frame_id = self.odomId
msg.child_frame_id = self.baseId
msg.pose.pose.position.x = self.pose_x
msg.pose.pose.position.y = self.pose_y
msg.pose.pose.position.z = 0.0
msg.pose.pose.orientation.x = pose_quat[0]
msg.pose.pose.orientation.y = pose_quat[1]
msg.pose.pose.orientation.z = pose_quat[2]
msg.pose.pose.orientation.w = pose_quat[3]
msg.twist.twist.linear.x = Vx
msg.twist.twist.angular.z = Vyaw
#print "Odometry pub success~"
for i in range(36):
msg.twist.covariance[i] = 0
msg.twist.covariance[0] = self.VxCov
msg.twist.covariance[35] = self.VyawCov
msg.pose.covariance = msg.twist.covariance
self.pub.publish(msg)
#print "pub success~"
# TF Broadcaster
if self.pub_tf:
self.tf_broadcaster.sendTransform(
(self.pose_x, self.pose_y, 0.0), pose_quat,
self.current_time, self.baseId, self.odomId)
except:
rospy.loginfo("Error in encoder value !")
pass
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
if self.debugPID:
rospy.logdebug("poll start------------------------")
try:
left_pidin, right_pidin = self.arduino.get_pidin()
self.lEncoderPub.publish(left_pidin)
self.rEncoderPub.publish(right_pidin)
rospy.logdebug("left_pidin: " + str(left_pidin))
rospy.logdebug("right_pidin: " + str(right_pidin))
except:
rospy.logerr("getpidout exception count: ")
return
try:
left_pidout, right_pidout = self.arduino.get_pidout()
self.lPidoutPub.publish(left_pidout)
self.rPidoutPub.publish(right_pidout)
rospy.logdebug("left_pidout: " + str(left_pidout))
rospy.logdebug("right_pidout: " + str(right_pidout))
except:
rospy.logerr("getpidout exception count: ")
return
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
if self.debugPID:
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
#!/usr/bin/env python
# license removed for brevity
import rospy
import math
import tf
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
initial_pose = Odometry()
target_pose = Odometry()
target_distance = 0
actuator_vel = 15
Ianterior = 0
rate_value = 10
Kp = 10
Ki = 2
def get_pose(initial_pose_tmp):
global initial_pose
initial_pose = initial_pose_tmp
def get_target(target_pose_tmp):
global target_pose
target_pose = target_pose_tmp
def thruster_ctrl_msg():
global actuator_vel
def pose(self):
last_odom_msg = self._odom_sub.get_last_message()
if self.test_mode:
last_odom_msg = Odometry() # All 0's
pose = pose_editor.PoseEditor.from_Odometry(last_odom_msg)
return pose
def __init__(self):
rospy.loginfo(rospy.get_name() + ": Start")
# defines
self.count = 0
# static parameters
self.update_rate = 50 # set update frequency [Hz]
self.deadman_tout_duration = 0.2 # [s]
self.cmd_vel_tout_duration = 0.2 # [s]
# get parameters
self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 500)
self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 500)
acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2]
acc_ang_max = rospy.get_param("~max_angular_acceleration", 1.8) # [rad/s^2]
self.wheel_speed_variance = 0.01
self.wheel_speed_delay = 0.1 # [s]
self.wheel_speed_delay_variance = 0.05
self.wheel_speed_error = 0.25 # [m/s]
self.wheel_speed_minimum = 0.07
pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0)
if pub_fb_rate != 0:
self.pub_fb_interval = int(self.update_rate/pub_fb_rate)
else:
self.pub_fb_interval = 0
# get topic names
self.topic_deadman = rospy.get_param("~deadman_sub",'/fmCommand/deadman')
self.topic_cmd_vel = rospy.get_param("~cmd_vel_sub",'/fmCommand/cmd_vel')
self.topic_odom_reset = rospy.get_param("~odom_reset_sub",'/fmInformation/odom_reset')
self.topic_pose = rospy.get_param("~pose_pub",'/fmKnowledge/pose')
self.topic_w_fb_left_pub = rospy.get_param("~wheel_feedback_left_pub",'/fmInformation/wheel_feedback_left')
self.topic_w_fb_right_pub = rospy.get_param("~wheel_feedback_right_pub",'/fmInformation/wheel_feedback_right')
# initialize internal variables
self.update_interval = 1/(self.update_rate * 1.0)
self.pi2 = 2*pi
self.cmd_vel_tout_active = True
self.pose = [0.0, 0.0, 0.0]
self.deadman_tout = 0.0
self.cmd_vel_msgs = []
self.vel_lin_desired = 0.0
self.vel_ang_desired = 0.0
self.acc_lin_max_step = acc_lin_max*self.update_interval
self.acc_ang_max_step = acc_ang_max*self.update_interval
self.vel_lin = 0.0
self.vel_ang = 0.0
self.ref_vel_left = 0.0
self.ref_vel_right = 0.0
self.sim_vel_left = 0.0
self.sim_vel_right = 0.0
self.wheel_speed_err_left = 0.0
self.wheel_speed_err_right = 0.0
self.dk = differential_kinematics(self.w_dist)
# setup topic publishers
self.pose_pub = rospy.Publisher(self.topic_pose, Odometry)
self.pose_msg = Odometry()
# setup wheel feedback topic publisher
if self.pub_fb_interval > 0:
self.wl_fb_vel_set = 0.0
self.wr_fb_vel_set = 0.0
self.w_fb_left_pub = rospy.Publisher(self.topic_w_fb_left_pub, PropulsionModuleFeedback)
self.w_fb_right_pub = rospy.Publisher(self.topic_w_fb_right_pub, PropulsionModuleFeedback)
self.w_fb = PropulsionModuleFeedback()
# setup subscription topic callbacks
rospy.Subscriber(self.topic_deadman, Bool, self.on_deadman_message)
rospy.Subscriber(self.topic_cmd_vel, TwistStamped, self.on_cmd_vel_message)
rospy.Subscriber(self.topic_odom_reset, FloatArrayStamped, self.on_odom_reset_message)
# call updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
import matplotlib.pyplot as plt
from scipy import interpolate
from autominy_msgs.msg import NormalizedSpeedCommand, NormalizedSteeringCommand, SteeringFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovariance, Pose, Point, PointStamped
from visualization_msgs.msg import Marker
from std_msgs.msg import Header, Int32
from sensor_msgs.msg import LaserScan
data = np.load('lane1.npy')
datab = np.load('lane2.npy')
print len(data[:, 0])
lane = 1
timecounter = 0
aktuellerpunkt = Odometry()
isactiv = False
f = interpolate.CubicSpline(data[:, 0], data[:, 1])
f2 = interpolate.CubicSpline(data[:, 0], data[:, 2])
fb = interpolate.CubicSpline(datab[:, 0], datab[:, 1])
f2b = interpolate.CubicSpline(datab[:, 0], datab[:, 2])
xnew = np.arange(0, 12.8, 0.1)
ynew = f(xnew)
xnewb = np.arange(0, 14.8, 0.1)
ynewb = fb(xnewb)
#plt.plot(data[:,0], data[:,1], 'o', xnew, ynew, '-')
#plt.show()
root = Tk()
root.title("Bebop Interface")
mainframe = ttk.Frame(root, padding="3 3 12 12")
mainframe.grid(column=0, row=0, sticky=(N, W, E, S))
mainframe.columnconfigure(0, weight=1)
rospy.init_node('BebopGUI', anonymous=False)
takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=1)
x_odom = StringVar()
y_odom = StringVar()
z_odom = StringVar()
yaw_odom = StringVar()
odom_var = Odometry()
def receive_height(data):
global height
height = data.altitude
z_odom.set("{0:.2f}".format(height))
def receive_attitude(data):
global att_p
att_p = math.degrees(data.yaw)
if (att_p >= -179.99 and att_p <= -0.1):
#att_p = 360 + att_p
att_p = att_p * -1
else:
#! /usr/bin/env python
import rospy
import math
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3
from std_msgs.msg import Float64
from nav_msgs.msg import Odometry
import roslib
import actionlib
import tbug.msg
robotpos = Odometry()
def robotposcheck(updatedState):
global robotpos
robotpos = updatedState
class GoalServer(object):
global robotpos
_feedback = tbug.msg.goalStatusFeedback()
_result = tbug.msg.goalStatusResult()
def __init__(self):
self._as = actionlib.SimpleActionServer('check_goals',
tbug.msg.goalStatusAction,
def spin(self):
encoders = [0, 0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
then = rospy.Time.now() + rospy.Duration(1.0)
# things that don't ever change
# scan_link = rospy.get_param('~frame_id', 'base_laser_link')
# scan = LaserScan(header=rospy.Header(frame_id=scan_link))
# scan.angle_min =0.0
# scan.angle_max =359.0*pi/180.0
# scan.angle_increment =pi/180.0
# scan.range_min = 0.020
# scan.range_max = 5.0
odom = Odometry(header=rospy.Header(frame_id=self.odom_frame),
child_frame_id=self.base_frame)
dist = Vector3Stamped(header=rospy.Header(frame_id=self.odom_frame))
# button = Button()
# sensor = Sensor()
self.robot.setBacklight(1)
self.robot.setLED("Green")
# main loop of driver
r = rospy.Rate(10)
cmd_rate = self.CMD_RATE
self.prevRanges = []
while not rospy.is_shutdown():
# notify if low batt
#if self.robot.getCharger() < 10:
# print "battery low " + str(self.robot.getCharger()) + "%"
# get motor encoder values
left, right = self.robot.getMotors()
cmd_rate = cmd_rate - 1
if cmd_rate == 0:
# send updated movement commands
#if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
# max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
#self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
self.robot.setMotors(
self.cmd_vel[0], self.cmd_vel[1],
max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
cmd_rate = self.CMD_RATE
self.old_vel = self.cmd_vel
# prepare laser scan
# scan.header.stamp = rospy.Time.now()
# self.robot.requestScan()
# scan.ranges = self.robot.getScanRanges()
# # now update position information
# dt = (scan.header.stamp - then).to_sec()
# then = scan.header.stamp
# compute delta time
dt = (rospy.Time.now() - then).to_sec()
then = rospy.Time.now()
d_left = (left - encoders[0]) / 1000.0
d_right = (right - encoders[1]) / 1000.0
encoders = [left, right]
#print d_left, d_right, encoders
dx = (d_left + d_right) / 2
dth = (d_right - d_left) / (self.robot.base_width / 1000.0)
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += cos(self.th) * x - sin(self.th) * y
self.y += sin(self.th) * x + cos(self.th) * y
self.th += dth
#print self.x,self.y,self.th
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# prepare odometry
# synchronize /odom and /scan in time
#odom.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
# no need to synchronize /odom and /scan
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.angular.z = dth / dt
dist.header.stamp = odom.header.stamp
dist.vector.x = encoders[0] / 1000
dist.vector.y = encoders[1] / 1000
# sensors
# lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()
#
# # buttons
# btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()
#
#
# publish everything
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
self.base_frame, self.odom_frame)
# if self.prevRanges != scan.ranges:
# self.scanPub.publish(scan)
# self.prevRanges = scan.ranges
self.odomPub.publish(odom)
self.distPub.publish(dist)
# button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
# sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
# for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
# if b == 1:
# button.value = b
# button.name = button_enum[idx]
# self.buttonPub.publish(button)
#
# for idx, b in enumerate((lsb, rsb, lfb, rfb)):
# if b == 1:
# sensor.value = b
# sensor.name = sensor_enum[idx]
# self.sensorPub.publish(sensor)
# wait, then do it again
r.sleep()
# shut down
self.robot.setBacklight(0)
self.robot.setLED("Off")
self.robot.setLDS("off")
self.robot.setTestMode("off")
print 'Warming up the IMU...'
rospy.init_node('imu_node')
rate = rospy.Rate(100)
br = tf.TransformBroadcaster()
pubTime = rospy.Publisher('/imu/timestamp', UInt32, queue_size=10)
pubAccAng = rospy.Publisher('/imu/AccAndAng', Twist, queue_size=10)
pubMag = rospy.Publisher('/imu/mag', Vector3, queue_size=10)
pubOdom = rospy.Publisher('/odom', Odometry, queue_size=10)
ser = serial.Serial('/dev/razor')
time.sleep(30) #Time for the IMU to warm up
print 'Done! Starting to publish...'
msgT = UInt32()
msgAA = Twist()
msgM = Vector3()
msgO = Odometry()
x = 0 #0.2475
y = 0 #-0.057
msgO.header.frame_id = 'odom'
msgO.child_frame_id = 'base_frame'
scale = 1
fixScale = True
while (True):
ser.flushInput()
string_list = (ser.read(150))
values_string = string_list
values_list = values_string.split(', ')
def ros():
rospy.init_node('simulator_gui_node')
a = rospy.Service('simulator_robot_step', simulator_robot_step,
handle_robot_step)
b = rospy.Service('simulator_print_graph', simulator_algorithm_result,
handle_print_graph)
c = rospy.Service('simulator_stop', simulator_stop, handle_simulator_stop)
d = rospy.Service('simulator_set_light_position',
simulator_set_light_position,
handle_simulator_set_light_position)
e = rospy.Service('simulator_object_interaction',
simulator_object_interaction,
handle_simulator_object_interaction)
#rospy.Subscriber('/scan',LaserScan,update_value,queue_size=1)
#rospy.Subscriber('/odom',Odometry, turtle_odometry ,queue_size=1)
odom_pub = rospy.Publisher("/odom_simul", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
x = 0.0
y = 0.0
th = 0.0
vx = 0.1
vy = -0.1
vth = 0.1
current_time = rospy.Time.now()
last_time = rospy.Time.now()
pub_params = rospy.Publisher('simulator_parameters_pub',
Parameters,
queue_size=0)
#rospy.Subscriber("simulator_laser_pub", Laser_values, callback)
msg_params = Parameters()
rate = rospy.Rate(100)
while not gui.stopped:
parameters = gui.get_parameters()
msg_params.robot_x = parameters[0]
msg_params.robot_y = parameters[1]
msg_params.robot_theta = parameters[2]
msg_params.robot_radio = parameters[3]
msg_params.robot_max_advance = parameters[4]
msg_params.robot_turn_angle = parameters[5]
msg_params.laser_num_sensors = parameters[6]
msg_params.laser_origin = parameters[7]
msg_params.laser_range = parameters[8]
msg_params.laser_value = parameters[9]
msg_params.world_name = parameters[10]
msg_params.noise = parameters[11]
msg_params.light_x = parameters[12]
msg_params.light_y = parameters[13]
msg_params.run = parameters[14]
msg_params.behavior = parameters[15]
msg_params.steps = parameters[16]
msg_params.turtle = parameters[16]
pub_params.publish(msg_params)
x = parameters[0]
y = parameters[1]
th = parameters[2]
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time,
"base_link_rob2w", "map")
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "map"
odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
odom.child_frame_id = "base_link_rob2w"
odom.twist.twist = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0))
odom_pub.publish(odom)
rate.sleep()
#print(gui.stopped)
for _ in range(20):
msg_params.run = False
pub_params.publish(msg_params)
rate.sleep()
class SensorOdomManager(object):
# ros node frequency
main_rate = 10
# input message definition for sensor
odom_in_msg = Odometry()
# output message definition
odom_msg = Odometry()
# tf message definition
tf_msg = TransformStamped()
# initialization function
def __init__(self):
# param fetch
self.odom_in_topic = get_param('~odom_in_topic', "/in_odom")
self.odom_out_topic = get_param('~odom_out_topic', "/odom")
self.odom_out_frame = get_param('~odom_out_frame', "odom")
self.base_frame = get_param('~base_frame', "base_link")
self.odom_calc_hz = get_param('~odom_calc_hz', 10)
self.sensor_offset = json.loads(
get_param('~sensor_offset', '{"x": 0., "y": 0., "z": 0.}'))
self.sensor_rotation = json.loads(
get_param('~sensor_rotation', '{"x": 0., "y": 0., "z": 0.}'))
self.ignore_x = get_param('~ignore_x', 'False')
self.ignore_y = get_param('~ignore_y', 'False')
self.ignore_z = get_param('~ignore_z', 'False')
# shutdown handling
rospy.on_shutdown(self.terminate)
# ros publishers
self.odom_publisher = rospy.Publisher(self.odom_out_topic,
Odometry,
tcp_nodelay=True,
queue_size=2)
self.odom_subscriber = rospy.Subscriber(self.odom_in_topic, Odometry,
self.sensor_odom_callback)
# setup qaternion
self.qtAdjustedRot = np.array([0, 0, 0, 1])
# setup input message from sensor
self.odom_msg.header.frame_id = self.odom_out_frame
self.odom_msg.child_frame_id = self.base_frame
self.odom_msg.pose.pose.position.x = 0.0
self.odom_msg.pose.pose.position.y = 0.0
self.odom_msg.pose.pose.position.z = 0.0
self.odom_msg.pose.pose.orientation.x = 0.0
self.odom_msg.pose.pose.orientation.y = 0.0
self.odom_msg.pose.pose.orientation.z = 0.0
self.odom_msg.pose.pose.orientation.w = 1.0
self.odom_msg.twist.twist.linear.x = 0.0
self.odom_msg.twist.twist.linear.y = 0.0
self.odom_msg.twist.twist.linear.z = 0.0
self.odom_msg.twist.twist.angular.x = 0.0
self.odom_msg.twist.twist.angular.y = 0.0
self.odom_msg.twist.twist.angular.z = 0.0
# setup transform
self.tf_publisher = tf2_ros.TransformBroadcaster()
self.tf_msg.header.frame_id = self.odom_out_frame
self.tf_msg.child_frame_id = self.base_frame
self.tf_msg.transform.translation.x = 0.0
self.tf_msg.transform.translation.y = 0.0
self.tf_msg.transform.translation.z = 0.0
self.tf_msg.transform.rotation.x = 0.0
self.tf_msg.transform.rotation.y = 0.0
self.tf_msg.transform.rotation.w = 0.0
self.tf_msg.transform.rotation.z = 1.0
# bool for flagging the first msg received from sensor
self.first_frame_received = False
# calback function of the sensor input topic
def sensor_odom_callback(self, data):
# mark the flag that the sensor message was received
self.first_frame_received = True
# get data from the sensor msg
self.odom_in_msg.header.frame_id = data.header.frame_id
self.odom_in_msg.child_frame_id = data.child_frame_id
self.odom_in_msg.pose.pose.position.x = data.pose.pose.position.x
self.odom_in_msg.pose.pose.position.y = data.pose.pose.position.y
self.odom_in_msg.pose.pose.position.z = data.pose.pose.position.z
self.odom_in_msg.pose.pose.orientation.x = data.pose.pose.orientation.x
self.odom_in_msg.pose.pose.orientation.y = data.pose.pose.orientation.y
self.odom_in_msg.pose.pose.orientation.z = data.pose.pose.orientation.z
self.odom_in_msg.pose.pose.orientation.w = data.pose.pose.orientation.w
self.odom_in_msg.twist.twist.linear.x = data.twist.twist.linear.x
self.odom_in_msg.twist.twist.linear.y = data.twist.twist.linear.y
self.odom_in_msg.twist.twist.linear.z = data.twist.twist.linear.z
self.odom_in_msg.twist.twist.angular.x = data.twist.twist.angular.x
self.odom_in_msg.twist.twist.angular.y = data.twist.twist.angular.y
self.odom_in_msg.twist.twist.angular.z = data.twist.twist.angular.z
# odometry publisher function
def pub_odometry(self, time_now):
# in case we received at least one frame from the sensor
if (self.first_frame_received == True):
# update timestapm
now = time_now
self.odom_msg.header.stamp = now
self.tf_msg.header.stamp = now
# update frame ids
self.odom_msg.header.frame_id = self.odom_out_frame
self.odom_msg.child_frame_id = self.base_frame
# Twist/velocity:
self.odom_msg.twist.twist.linear.x = self.odom_in_msg.twist.twist.linear.x
self.odom_msg.twist.twist.angular.z = self.odom_in_msg.twist.twist.linear.z
# get sensor pose quaternion
self.qtAdjustedRot = np.array([
self.odom_in_msg.pose.pose.orientation.x,
self.odom_in_msg.pose.pose.orientation.y,
self.odom_in_msg.pose.pose.orientation.z,
self.odom_in_msg.pose.pose.orientation.w
])
# in case there is no configured rotation to the sensor, ignore the quaternion calc
if (self.sensor_rotation["x"] != 0.
or self.sensor_rotation["y"] != 0.
or self.sensor_rotation["z"] != 0.):
# calc quaternion rotation tool (angles are negative because we need to adjust the sensor from the config data )
qtSensorAdjustmentTool = tf.transformations.quaternion_from_euler(
np.radians(-self.sensor_rotation["x"]),
np.radians(-self.sensor_rotation["y"]),
np.radians(-self.sensor_rotation["z"]))
# rotate sensor tf
qtRot = tf.transformations.quaternion_multiply(
qtSensorAdjustmentTool, self.qtAdjustedRot)
self.qtAdjustedRot = tf.transformations.quaternion_multiply(
qtRot,
tf.transformations.quaternion_inverse(qtSensorAdjustment))
# get the euler angles from pose quaternion
self.rpy = tf.transformations.euler_from_quaternion(
self.qtAdjustedRot)
# calculate translations
# check if we need to calculate or ignore x coordonate
if (self.ignore_x == False):
# calculate x coordonate
self.transl_x = self.odom_in_msg.pose.pose.position.x - (
(self.sensor_offset["x"] * math.cos(self.rpy[2])) -
(-self.sensor_offset["y"] * math.sin(self.rpy[2])))
else:
# ignore x coordonate of the sensor, therefore set it to 0
self.transl_x = 0
# check if we need to calculate or ignore y coordonate
if (self.ignore_y == False):
# calculate y coordonate
self.transl_y = self.odom_in_msg.pose.pose.position.y - (
(self.sensor_offset["x"] * math.sin(self.rpy[2])) +
(-self.sensor_offset["y"] * math.cos(self.rpy[2])))
else:
# ignore y coordonate of the sensor, therefore set it to 0
self.transl_y = 0
# check if we need to calculate or ignore z coordonate
if (self.ignore_z == False):
# calculate z coordonate
self.transl_z = self.odom_in_msg.pose.pose.position.z - self.sensor_offset[
"z"]
else:
# ignore z coordonate of the sensor, therefore set it to 0
self.transl_z = 0
# update odom message
self.odom_msg.pose.pose.position.x = self.transl_x
self.odom_msg.pose.pose.position.y = self.transl_y
self.odom_msg.pose.pose.position.z = self.transl_z
self.odom_msg.pose.pose.orientation.x = self.qtAdjustedRot[0]
self.odom_msg.pose.pose.orientation.y = self.qtAdjustedRot[1]
self.odom_msg.pose.pose.orientation.z = self.qtAdjustedRot[2]
self.odom_msg.pose.pose.orientation.w = self.qtAdjustedRot[3]
# update tf
self.tf_msg.transform.translation.x = self.transl_x
self.tf_msg.transform.translation.y = self.transl_y
self.tf_msg.transform.translation.z = self.transl_z
self.tf_msg.transform.rotation.x = self.qtAdjustedRot[0]
self.tf_msg.transform.rotation.y = self.qtAdjustedRot[1]
self.tf_msg.transform.rotation.z = self.qtAdjustedRot[2]
self.tf_msg.transform.rotation.w = self.qtAdjustedRot[3]
# ... and publish!
self.tf_publisher.sendTransform(self.tf_msg)
self.odom_publisher.publish(self.odom_msg)
# shutdown hook handler
def terminate(self):
# shutdown ros timer
self.fast_timer.shutdown()
# scheduling timer handler
def fast_timer(self, timer_event):
time_now = rospy.Time.now()
self.pub_odometry(time_now)
# main handler
def main_loop(self):
# Main control, handle startup and error handling
# while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs
self.main_rate = rospy.Rate(10) # hz
# Start timer to run high-rate comms
self.fast_timer = rospy.Timer(
rospy.Duration(1 / float(self.odom_calc_hz)), self.fast_timer)
#Locatio
ns = [
location("Kitchen", 0.59, -2.05, 0.969, -0.247),
location("TV", 2.06, 3.84, 0.908, -0.42),
location("Desk", -0.22, 0.198, -0.0345, 1),
location("Dinning", -3.8, 0.2715, 0, 1)
]
#Hardcode with location=(x,y) and rotation = (z,w)
client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
current_AMCL_pose = PoseWithCovarianceStamped()
current_Odom_pose = Odometry()
current_state = CurrentState(0, 0)
#record what amcl is posting
#A estimation of the current pose
root = Tk()
#ROOT is the whole window
topFrame = Frame(root)
topFrame.pack(side=TOP)
downFrame = Frame(root)
downFrame.pack(side=BOTTOM)
#Down Row for info about navigation
leftFrame = Frame(topFrame)
leftFrame.pack(side=LEFT)
#Left Column for info about the current location
middleFrame = Frame(topFrame)
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
vy = float(lineParts[1])
omega = float(lineParts[1])
self.batteryVoltage = float(lineParts[15]) * 0.0287
#BatteryState = BatteryState()
#BatteryState = batteryVoltage * 0.287
# calculate position
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_link", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.pose.covariance = [
1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0,
0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3
]
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = vy
odometry.twist.twist.angular.z = omega
odometry.twist.covariance = [
1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0,
0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3
]
self._OdometryPublisher.publish(odometry)
#self._BatteryStatePublisher.publish(12)
#rospy.loginfo(self.batteryVoltage)
except:
rospy.logwarn("odom Unexpected error:" + str(sys.exc_info()[0]))
robot_yaw = 0
robot_pos_x = 0
robot_pos_y = 0
publishCount = 0
printCount = 0
# ROS subscriber, publisher and broadcaster setup:
rospy.init_node('neato_driver_rpi')
velocity_inputs = CallbackHandler()
rospy.Subscriber('cmd_vel', Twist, velocity_inputs.handle_input)
odom_broadcaster = tf.TransformBroadcaster()
odom_publisher = rospy.Publisher('odom', Odometry, queue_size=50)
odom = Odometry()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_footprint'
try:
while not rospy.is_shutdown():
# Convert from robot velocity to wheel spin velocities:
angularRef = velocity_inputs.robotAngularRef * angular2wheel
linearRef = velocity_inputs.robotLinearRef * linear2wheel
right_ref = linearRef + angularRef
left_ref = linearRef - angularRef
# In case the combination of the linear and angular references become too fast
# for the wheels to cope with, the following condition will reduce the wheel speed
# to the value defined in variable MAX_WHEEL_VEL while keeping the ratio between
def odom_cb(self, odom_data):
'''
Converts OdometrySmall to Odometry message and publishes to odom topic
'''
dt = odom_data.dt / 1000000.0 # Delta time converted to seconds
msg = Odometry()
msg.header.stamp = odom_data.header.stamp
msg.header.frame_id = "odom"
msg.child_frame_id = "base_link"
# Foward kinematics
v_left = odom_data.rps1 / self.rot_per_m_right # Wheel speed in m/s
v_right = odom_data.rps2 / self.rot_per_m_left # Wheel speed in m/s
vx = (v_right + v_left
) / 2 * self.odom_pitch_correction_coef # Speed forward in m/s
omega = (v_right - v_left) / self.base_width # Angular speed in rad/s
# Clamped angle
self.theta += omega * dt
if self.theta > math.pi:
self.theta -= 2 * math.pi
elif self.theta < -math.pi:
self.theta += 2 * math.pi
# print("Theta: %f, Omega %f" %(self.theta, omega))
# print("vL: %f, vR %f" %(v_left, v_right))
# Position
self.posx += vx * math.cos(self.theta) * dt
self.posy += vx * math.sin(self.theta) * dt
msg.pose.pose.position = Point(self.posx, self.posy, 0) # x - fwd
# Angle
quat = quaternion_from_euler(0, 0, self.theta)
msg.pose.pose.orientation.x = quat[0]
msg.pose.pose.orientation.y = quat[1]
msg.pose.pose.orientation.z = quat[2]
msg.pose.pose.orientation.w = quat[3]
# Linear speed
msg.twist.twist.linear.x = vx
# msg.twist.twist.linear.y = 0 # Default already zero
# Angular speed
msg.twist.twist.angular.z = omega
# Covariance | Make diagonal covariance matrix
default_cov = [
0.0,
] * 36
for i in range(0, 6):
default_cov[i + i * 6] = 0.00001 # NOTE: tuning robot localization
msg.twist.covariance = tuple(default_cov)
msg.pose.covariance = tuple(default_cov)
pub = rospy.Publisher("odom", Odometry, queue_size=10)
pub.publish(msg)
if self.debug is True:
self.calibration_debug_data(odom_data)
# ------------
rospy.Subscriber("green_robot/encoderCount/left", Int32Stamped,
callBackLeftEncoderCount)
rospy.Subscriber("green_robot/encoderCount/right", Int32Stamped,
callBackRightEncoderCount)
# main node loop
# ---------------
# -----------------------------------------------------------------------------
if __name__ == '__main__':
# -----------------------------------------------------------------------------
#rospy.spin()
#global robot
odomMsg = Odometry()
odomMsg.header.frame_id = 'world'
odomMsg.child_frame_id = 'robot'
t = rospy.get_time()
tPrec = t
leftWheelTheta = 0.0
leftWheelThetaPrec = 0.0
rightWheelTheta = 0.0
rightWheelThetaPrec = 0.0
rL = robot.leftWheel.diameter / 2.0
rR = robot.rightWheel.diameter / 2.0
quaternion = Quaternion()
#rospy.loginfo("robot x=%f y=%f theta=%f rL=%f rR=%f res=%f=%f" % (robot.x, robot.y, robot.theta, rL, rR, robot.leftWheel.encoderResolution, robot.rightWheel.encoderResolution))
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
try:
r1, r2, r3, r4 = self.arduino.ping()
self.left_ranger = r1
self.front_ranger = r2
self.right_ranger = r3
self.back_ranger = r4
rospy.loginfo("ranger: " + str(r1) + "," + str(r2) + "," +
str(r3) + "," + str(r4))
except:
rospy.logerr("ping error")
return
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
#rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
if (left_enc < self.encoder_low_wrap
and self.enc_left > self.encoder_high_wrap):
self.l_wheel_mult = self.l_wheel_mult + 1
elif (left_enc > self.encoder_high_wrap
and self.enc_left < self.encoder_low_wrap):
self.l_wheel_mult = self.l_wheel_mult - 1
else:
self.l_wheel_mult = 0
if (right_enc < self.encoder_low_wrap
and self.enc_right > self.encoder_high_wrap):
self.r_wheel_mult = self.r_wheel_mult + 1
elif (right_enc > self.encoder_high_wrap
and self.enc_right < self.encoder_low_wrap):
self.r_wheel_mult = self.r_wheel_mult - 1
else:
self.r_wheel_mult = 0
#dright = (right_enc - self.enc_right) / self.ticks_per_meter
#dleft = (left_enc - self.enc_left) / self.ticks_per_meter
dleft = 1.0 * (left_enc + self.l_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_left) / self.ticks_per_meter
dright = 1.0 * (right_enc + self.r_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_right) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def calcodom(data):
global forward
global rwb_last_rotation
global lwb_last_rotation
global x_pos
global y_pos
global th
global x_orientation
global y_orientation
global z_orientation
global w_orientation
global last_time
current_time = rospy.Time.now()
time_diff = (current_time - last_time).nsecs
last_time = current_time
odom_broadcaster = tf.TransformBroadcaster()
#rospy.loginfo(data.transforms[0])
if data.transforms[0].child_frame_id == "left_steering" and time_diff != 0:
lwb = data.transforms[1]
rwb = data.transforms[4]
#rospy.loginfo(lwb.transform.rotation.y)
#rospy.loginfo(rwb.transform.rotation.y)
lwb_diff = lwb.transform.rotation.y - lwb_last_rotation
rwb_diff = rwb.transform.rotation.y - rwb_last_rotation
if lwb_diff < 0.01 and lwb_diff > -0.01:
lwb_diff = 0
if rwb_diff < 0.01 and rwb_diff > -0.01:
rwb_diff = 0
if lwb_diff < -0.5 and forward == 1:
#rospy.loginfo("lwbbefore %f",lwb_diff)
#rospy.loginfo("lwb %f",lwb_last_rotation)
#rospy.loginfo("lwb %f",lwb.transform.rotation.y)
lwb_diff = 2 + lwb.transform.rotation.y - lwb_last_rotation
#rospy.loginfo("lwb %f",lwb_diff)
if rwb_diff < -0.5 and forward == 1:
#rospy.loginfo("rwbbefore %f",rwb_diff)
#rospy.loginfo("rwb %f",rwb_last_rotation)
#rospy.loginfo("rwb %f",rwb.transform.rotation.y)
rwb_diff = 2 + rwb.transform.rotation.y - rwb_last_rotation
#rospy.loginfo("rwb %f",rwb_diff)
if lwb_diff > 0.5 and forward == 0:
#rospy.loginfo("lwbbefore %f",lwb_diff)
#rospy.loginfo("lwb %f",lwb_last_rotation)
#rospy.loginfo("lwb %f",lwb.transform.rotation.y)
lwb_diff = 2 - lwb.transform.rotation.y + lwb_last_rotation
#rospy.loginfo("lwb %f",lwb_diff)
if rwb_diff > 0.5 and forward == 0:
#rospy.loginfo("rwbbefore %f",rwb_diff)
#rospy.loginfo("rwb %f",rwb_last_rotation)
#rospy.loginfo("rwb %f",rwb.transform.rotation.y)
rwb_diff = 2 - rwb.transform.rotation.y + rwb_last_rotation
#rospy.loginfo("rwb %f",rwb_diff)
lwb_diff = lwb_diff * 2 * 0.05 * math.pi
rwb_diff = rwb_diff * 2 * 0.05 * math.pi
#rospy.loginfo(lwb_diff)
#rospy.loginfo(rwb_diff)
#rospy.loginfo("")
lwb_last_rotation = lwb.transform.rotation.y
rwb_last_rotation = rwb.transform.rotation.y
vel_left = lwb_diff / time_diff
vel_right = rwb_diff / time_diff
vel_x = (vel_left + vel_right / 2)
vel_y = 0
vel_th = (vel_right - vel_left) / 0.325
delta_x = (vel_x * math.cos(th)) * time_diff
delta_y = (vel_x * math.sin(th)) * time_diff
delta_th = vel_th * time_diff
x_pos += delta_x
y_pos += delta_y
th += delta_th
rospy.loginfo("xpos at %f", x_pos)
rospy.loginfo("ypos at %f", y_pos)
rospy.loginfo("th at %f", th)
rospy.loginfo("lwbdiff at %f", lwb_diff)
rospy.loginfo("rwbdiff at %f", rwb_diff)
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
x_orientation = odom_quat[0]
y_orientation = odom_quat[1]
z_orientation = odom_quat[2]
w_orientation = odom_quat[3]
quat = Quaternion(x_orientation, y_orientation, z_orientation,
w_orientation)
odom_trans = TransformStamped()
odom_trans.header.stamp = current_time
odom_trans.header.frame_id = "odom"
odom_trans.child_frame_id = "base_link"
odom_trans.transform.translation.x = x_pos
odom_trans.transform.translation.y = y_pos
odom_trans.transform.translation.z = 0.0
#odom_trans.transform.rotation = data.pose[1].orientation;
odom_broadcaster.sendTransform(
(x_pos, y_pos, 0.0),
(x_orientation, y_orientation, z_orientation, w_orientation),
current_time, "base_link", "odom")
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
odom.pose.pose.position.x = x_pos
odom.pose.pose.position.y = y_pos
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = quat
odom.pose.covariance[0] = 0.2
odom.pose.covariance[7] = 0.2
odom.pose.covariance[35] = 0.4
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = vel_x
odom.twist.twist.linear.y = vel_y
odom.twist.twist.angular.z = vel_th
pub.publish(odom)
frame_id = "mechROS/tf/odom" child_frame_id = "/mechROS_base_link" # Define topics raw_imu_topic = "/raw_imu" publish_imu_odometry_topic = "/IMU/odom" publish_imu_mag_topic = "imu/mag" publish_imu_topic = "/imu/data_raw" # For Magdwick filter convension imu_magdwick_topic = "/imu/data" velocity_command_topic = "/cmd_vel" # Po mereni smazat raw_imu_publish = "raw_data_imu" # Define static odometry information odom = Odometry() odom.header.frame_id = frame_id odom.child_frame_id = child_frame_id odom.pose.covariance = cov_pose odom.twist.covariance = cov_twist # Define static IMU information imu = Imu() imu.header.frame_id = frame_id imu.orientation_covariance = orientation_cov imu.angular_velocity_covariance = ang_vel_cov # Define static magnetic information mag = MagneticField() mag.header.frame_id = frame_id mag.magnetic_field_covariance = mag_field_cov
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
# license removed for brevity
import rospy
from geometry_msgs.msg import Twist, Accel
from nav_msgs.msg import Odometry
from math import atan2
#全局变量声明
flowfield_velocity_latest = Twist() #记录最后一个到来的流场数据
control_msg_latest = Accel() #记录最后一个到来的控制输入
expect_velocity = Twist() #记录当前的静场运动速度
ground_truth_latest = Odometry() #记录当前的实时状态
flowfield_direction_publisher = rospy.Publisher('flowfield_direction', Twist, queue_size=10)
def OnFlowfieldMsg(flowfield_velocity):
#全局变量声明
global flowfield_velocity_latest
global flowfield_direction_publisher
flowfield_velocity_latest = flowfield_velocity;
flowfield_direction = Twist();
flowfield_speed_square = (flowfield_velocity.linear.x * flowfield_velocity.linear.x + flowfield_velocity.linear.y * flowfield_velocity.linear.y)
flowfield_direction.linear.x=flowfield_velocity.linear.x / flowfield_speed_square
flowfield_direction.linear.y=flowfield_velocity.linear.y / flowfield_speed_square
flowfield_direction_publisher.publish(flowfield_direction)
def OnControlMsg(control_msg):
#全局变量声明
global control_msg_latest
control_msg_latest = control_msg
def spin(self):
# state
s = self.sensor_state
odom = Odometry(header=rospy.Header(frame_id=self.odom_frame), child_frame_id=self.base_frame)
js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"],
position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0])
r = rospy.Rate(self.update_rate)
last_cmd_vel = 0, 0
last_cmd_vel_time = rospy.get_rostime()
last_js_time = rospy.Time(0)
# We set the retry count to 0 initially to make sure that only
# if we received at least one sensor package, we are robust
# agains a few sensor read failures. For some strange reason,
# sensor read failures can occur when switching to full mode
# on the Roomba.
sensor_read_retry_count = 0
while not rospy.is_shutdown():
last_time = s.header.stamp
curr_time = rospy.get_rostime()
# SENSE/COMPUTE STATE
try:
self.sense(s)
transform = self.compute_odom(s, last_time, odom)
# Future-date the joint states so that we don't have
# to publish as frequently.
js.header.stamp = curr_time + rospy.Duration(1)
except select.error:
# packet read can get interrupted, restart loop to
# check for exit conditions
continue
except DriverError:
if sensor_read_retry_count > 0:
rospy.logwarn('Failed to read sensor package. %d retries left.' % sensor_read_retry_count)
sensor_read_retry_count -= 1
continue
else:
raise
sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT
# Reboot Create if we detect that charging is necessary.
# if s.charging_sources_available > 0 and \
# s.oi_mode == 1 and \
# s.charging_state in [0, 5] and \
# s.charge < 0.93*s.capacity:
# rospy.loginfo("going into soft-reboot and exiting driver")
# self._robot_reboot()
# rospy.loginfo("exiting driver")
# break
# Reboot Create if we detect that battery is at critical level switch to passive mode.
# if s.charging_sources_available > 0 and \
# s.oi_mode == 3 and \
# s.charging_state in [0, 5] and \
# s.charge < 0.15*s.capacity:
# rospy.loginfo("going into soft-reboot and exiting driver")
# self._robot_reboot()
# rospy.loginfo("exiting driver")
# break
# PUBLISH STATE
if transform is not None:
self.sensor_state_pub.publish(s)
self.odom_pub.publish(odom)
if self.publish_tf:
self.publish_odometry_transform(odom)
# 1hz, future-dated joint state
# if curr_time > last_js_time + rospy.Duration(1):
# self.joint_states_pub.publish(js)
# last_js_time = curr_time
# self._diagnostics.publish(s, self._gyro)
# if self._gyro:
# self._gyro.publish(s, last_time)
# ACT
if self.req_cmd_vel is not None:
# check for velocity command and set the robot into full mode if not plugged in
# if s.oi_mode != self.operate_mode and s.charging_sources_available != 1:
# if self.operate_mode == 2:
# self._robot_run_safe()
# else:
# self._robot_run_full()
# check for bumper contact and limit drive command
req_cmd_vel = self.check_bumpers(s, self.req_cmd_vel)
# Set to None so we know it's a new command
self.req_cmd_vel = None
# reset time for timeout
last_cmd_vel_time = last_time
else:
#zero commands on timeout
# if last_time - last_cmd_vel_time > self.cmd_vel_timeout:
# last_cmd_vel = 0,0
# double check bumpers
req_cmd_vel = self.check_bumpers(s, last_cmd_vel)
# if(req_cmd_vel[0] != last_cmd_vel[0] or req_cmd_vel[1] != last_cmd_vel[1]): # Added if only drive command are different then issue new command
# send command
if self.emergency_activated:
#self.drive_cmd(0, 0)
self.robot.sci.getSer().write('STOP 0' + chr(13))
else:
self.drive_cmd(*req_cmd_vel)
# record command
last_cmd_vel = req_cmd_vel
r.sleep()
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float32MultiArray
from nav_msgs.msg import Odometry
import numpy as np
from math import sin, cos, pi
last_data = Odometry()
started = False
pub = rospy.Publisher('/kumara/odom', Odometry, queue_size=1000)
#last_data.pose.pose.position.z = 0.0
#last_data.pose.pose.orientation.x = 0.0
#last_data.pose.pose.orientation.y = 0.0
def etoq(yaw):
yaw = (yaw * pi) / 180.00 #convert to rad
w = cos(yaw / 2)
z = sin(yaw / 2)
return [w, z]
def callback(data):
global started, last_data
last_data.header.frame_id = "odom"
last_data.child_frame_id = "base_footprint"
last_data.pose.pose.position.x = data.data[0]
last_data.pose.pose.position.y = data.data[1]
last_data.pose.pose.orientation.w = etoq(data.data[2])[0]
last_data.pose.pose.orientation.z = etoq(data.data[2])[1]
def __init__(self, topic_name='/odom'):
self._topic_name = topic_name
self._sub = rospy.Subscriber(self._topic_name, Odometry,
self.topic_callback)
self._odomdata = Odometry()
# Author : Abhishek Raj Dutta
# Date :22/11/2106
# This code merges stereo odometry with IMU and command velocities to produce an estimate of the HUSKY's pose and heading
import rospy
import tf.transformations
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
import math
import numpy as np
pub = rospy.Publisher("/odometry/filtered/self", Odometry, queue_size=10)
br = tf.TransformBroadcaster()
odom = Odometry()
imu = Imu()
yawViso = 0.0
vYawViso = 0.0
vX = 0.0
yawImu = 0.0
yawImu1 = 0.0
yaw = 0.0
i = 0
vYawCov = 0.09
ImuYawCov = 0.15707963267948966
yawCov = 10
cmd = np.array([(0.0, 0.0), (0.0, 0.0)])
yaw = np.array([0.0, 0.0])
meas = np.array([(0.0, 0.0), (0.0, 0.0)])
# meas = np.array ([0.0,0.0])
def poll(self):
self.send_debug_pid()
time_now = rospy.Time.now()
if time_now > self.t_next:
#rospy.logwarn("Voltage: %f, Current: %f", float(vol), float(current))
#vol = self.arduino.detect_voltage()
#current = self.arduino.detect_current()
#rospy.logwarn("Voltage: %f, Current: %f", vol, current)
# Read the encoders
try:
aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts(
)
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
#rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))
# Calculate odometry
dist_A = (aWheel_enc - self.enc_A) / self.ticks_per_meter
dist_B = (bWheel_enc - self.enc_B) / self.ticks_per_meter
dist_C = (cWheel_enc - self.enc_C) / self.ticks_per_meter
# save current encoder value for next calculate
self.enc_A = aWheel_enc
self.enc_B = bWheel_enc
self.enc_C = cWheel_enc
dt = time_now - self.then
self.then = time_now
dt = dt.to_sec()
va = dist_A / dt
vb = dist_B / dt
vc = dist_C / dt
#forward kinemation
vx = sqrt(3) * (va - vb) / 3.0
vy = (va + vb - 2 * vc) / 3.0
vth = (va + vb + vc) / (3 * self.wheel_track)
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# create the odometry transform frame broadcaster.
#self.odomBroadcaster.sendTransform(
# (self.x, self.y, 0),
# (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
# rospy.Time.now(),
# self.base_frame,
# self.odom_name
#)
odom = Odometry()
odom.header.frame_id = self.odom_name
odom.child_frame_id = self.base_frame
odom.header.stamp = time_now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.pose.covariance = [
1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
]
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
odom.twist.covariance = [
1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
]
self.odomPub.publish(odom)
# send motor cmd
if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
self.stop()
else:
self.send_motor_speed()
# set next compare time
self.t_next = time_now + self.t_delta
def controller(self):
rate = rospy.Rate(10) # 10hz
# self.x = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[0] # update self.x everytime
# self.y = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[1] # update self.x everytime
while not rospy.is_shutdown():
# for simulating
self.vx = self.twist.linear.x
self.vy = self.twist.linear.y
self.vth = self.twist.angular.z
self.current_time = rospy.Time.now()
# We do not need this part, we are doing our own localization
# compute odometry in a typical way given the velocities of the robot
self.dt = (self.current_time - self.last_time).to_sec()
self.delta_x = (self.vx * cos(self.th) -
self.vy * sin(self.th)) * self.dt
self.delta_y = (self.vx * sin(self.th) +
self.vy * cos(self.th)) * self.dt
self.delta_th = self.vth * self.dt
# from GPS
# self.target_location = [earth_radius, 41.103465, 29.027909] # publish to move_base_simple/goal
# from magnetometer
# self.yaw = 0
# X, Y = current location
# th = current yaw
"""
self.x += self.delta_x
self.y += self.delta_y
self.th += self.delta_th
"""
# since all odometry is 6DOF we'll need a quaternion created from yaw
self.odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.th)
# first, we'll publish the transform over tf
"""
self.odom_broadcaster.sendTransform(
(self.x, self.y, 0.),
self.odom_quat,
self.current_time,
"base_link",
"odom"
)
"""
# next, we'll publish the odometry message over ROS
self.odom = Odometry()
self.odom.header.stamp = self.current_time
self.odom.header.frame_id = "odom"
# set the position
self.odom.pose.pose = Pose(Point(self.x, self.y, 0.),
Quaternion(*self.odom_quat))
# Write a tranform formula for calculating linear.x linear.y and angular.z speed
# set the velocity
self.odom.child_frame_id = "base_link"
self.odom.twist.twist = Twist(Vector3(self.vx, self.vy, 0),
Vector3(0, 0, self.vth))
# publish the PoseWithCovarianceStamped
self.pwcs = PoseWithCovarianceStamped()
self.pwcs.header.stamp = self.odom.header.stamp
self.pwcs.header.frame_id = "odom"
self.pwcs.pose.pose = self.odom.pose.pose
self.last_time = self.current_time
# publish the NavSatFix
self.sensor_data = None
self.gps_fix = NavSatFix()
self.gps_fix.header.frame_id = "base_link"
self.gps_fix.header.stamp = self.odom.header.stamp
self.gps_fix.status.status = 0 # GPS FIXED
self.gps_fix.status.service = 1 # GPS SERVICE = GPS
# Buralar bizden gelecek
self.gps_fix.latitude = 41.24600
self.gps_fix.longitude = 29.123123
self.gps_fix.altitude = 0
self.gps_fix.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
self.gps_fix.position_covariance_type = 0
# publish the NavSatFix
self.waypoint = WayPoint()
# self.waypoint.id.uuid = [1]
self.waypoint.position.latitude = 41.24678
self.waypoint.position.longitude = 29.123100
self.waypoint.position.altitude = 0
# self.waypoint.props.key = "key"
# self.waypoint.props.value = "1"
# publish the NavSatFix
self.imuMsg = Imu()
self.imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
self.imuMsg.angular_velocity_covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.imuMsg.linear_acceleration_covariance = [
0, 0, 0, 0, 0, 0, 0, 0, 0
]
self.roll = 0
self.pitch = 0
self.yaw = self.th # şimdilik
# Acceloremeter
self.imuMsg.linear_acceleration.x = 0
self.imuMsg.linear_acceleration.y = 0
self.imuMsg.linear_acceleration.z = 0
# Gyro
self.imuMsg.angular_velocity.x = 0
self.imuMsg.angular_velocity.y = 0
self.imuMsg.angular_velocity.z = 0
self.q = tf.transformations.quaternion_from_euler(
self.roll, self.pitch, self.yaw)
self.imuMsg.orientation.x = self.q[0] #magnetometer
self.imuMsg.orientation.y = self.q[1]
self.imuMsg.orientation.z = self.q[2]
self.imuMsg.orientation.w = self.q[3]
self.imuMsg.header.frame_id = "base_link"
self.imuMsg.header.stamp = self.odom.header.stamp
# Subscriber(s)
rospy.Subscriber('/husky_velocity_controller/cmd_vel', Twist,
self.callback)
rospy.Subscriber('/odometry/gps', Odometry, self.gps_callback)
# Publisher(s)
self.odom_pub.publish(self.odom)
self.odom_combined_pub.publish(self.pwcs)
self.gps_pub.publish(self.gps_fix)
self.waypoint_pub.publish(self.waypoint)
self.imu_pub.publish(self.imuMsg)
rate.sleep()
th += delta_th
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform(
(x, y, 0.),
odom_quat,
current_time,
"base_link",
"odom"
)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
# publish the message
odom_pub.publish(odom)
last_time = current_time
r.sleep()
class CommandHandler(AbstractCommandHandler):
def __init__(self):
#initailizing the services and publishers/subscribers
rospy.wait_for_service('mavros/cmd/arming')
rospy.wait_for_service('mavros/cmd/takeoff')
rospy.wait_for_service('mavros/set_mode')
try:
self._armService = rospy.ServiceProxy('mavros/cmd/arming',
mavros_msgs.srv.CommandBool)
self._flightModeService = rospy.ServiceProxy(
'mavros/set_mode', mavros_msgs.srv.SetMode)
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
self._sp_pub = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
self._local_pos = Odometry()
self._taskname = None
self._rate = rospy.Rate(20.0)
self._state = State()
self._vision_pose = PoseStamped()
self._camera_pose = None
self.tfmessage = None
self._vision_pose_pub = rospy.Publisher("/mavros/vision_pose/pose",
PoseStamped,
queue_size=1000)
rospy.Subscriber('mavros/state', State, self.stateCb)
rospy.Subscriber('mavros/local_position/odom', Odometry, self.odomCb)
rospy.Subscriber("/tf", TFMessage, self.tfCb)
##---------COmpute the HT from imu-camera---------------#
translation_matrix = [-0.05, 0, 0.24638]
translation_matrix = np.array(translation_matrix)
translation_matrix = translation_matrix.reshape(1, 3)
rotation_angles = [
0.5 * np.pi / 180, 5 * np.pi / 180.0, 90 * np.pi / 180
]
alpha = rotation_angles[2]
beta = rotation_angles[1]
gamma = rotation_angles[0]
last_row = np.array([0.0, 0.0, 0.0, 1.0]).reshape(1, 4)
rotation_matrix = np.array(
[[
np.cos(alpha) * np.cos(beta),
np.cos(alpha) * np.sin(beta) * np.sin(gamma) -
np.sin(alpha) * np.cos(gamma),
np.cos(alpha) * np.sin(beta) * np.cos(gamma) +
np.sin(alpha) * np.sin(gamma)
],
[
np.sin(alpha) * np.cos(beta),
np.sin(alpha) * np.sin(beta) * np.sin(gamma) +
np.cos(alpha) * np.cos(gamma),
np.sin(alpha) * np.sin(beta) * np.cos(gamma) -
np.cos(alpha) * np.sin(gamma)
],
[
-np.sin(beta),
np.cos(beta) * np.sin(gamma),
np.cos(beta) * np.cos(gamma)
]])
self.ht = np.concatenate(
(rotation_matrix, translation_matrix.transpose()), axis=1)
self.ht = np.concatenate((self.ht, last_row), axis=0)
