def to_message(self):
"""Convert current data to a trajectory point message.
> *Returns*
Trajectory point message as `uuv_control_msgs/TrajectoryPoint`
"""
p_msg = TrajectoryPointMsg()
# FIXME Sometimes the time t stored is NaN
(secs, nsecs) = float_sec_to_int_sec_nano(self.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs).to_msg()
p_msg.pose.position = geometry_msgs.Point(x=self.p[0],
y=self.p[1],
z=self.p[2])
p_msg.pose.orientation = geometry_msgs.Quaternion(x=self.q[0],
y=self.q[1],
z=self.q[2],
w=self.q[3])
p_msg.velocity.linear = geometry_msgs.Vector3(x=self.v[0],
y=self.v[1],
z=self.v[2])
p_msg.velocity.angular = geometry_msgs.Vector3(x=self.w[0],
y=self.w[1],
z=self.w[2])
p_msg.acceleration.linear = geometry_msgs.Vector3(x=self.a[0],
y=self.a[1],
z=self.a[2])
p_msg.acceleration.angular = geometry_msgs.Vector3(x=self.alpha[0],
y=self.alpha[1],
z=self.alpha[2])
return p_msg
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
if self.config['odom_vel_in_world']:
# This is a temp. workaround for gazebo's pos3d plugin not behaving properly:
# Twist should be provided wrt child_frame, gazebo provides it wrt world frame
# see http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w])
q_wb = xyzw_array(msg.pose.pose.orientation)
R_bw = trans.quaternion_matrix(q_wb)[0:3, 0:3].transpose()
v_linear = R_bw.dot(v_linear)
v_angular = R_bw.dot(v_angular)
# Compute compute control output:
t = msg.header.stamp.to_sec()
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(*a_linear)
cmd_accel.angular = geometry_msgs.Vector3(*a_angular)
self.pub_cmd_accel.publish(cmd_accel)
def publish_pose(self):
'''TODO:
Publish velocity in body frame
'''
linear_vel = self.body.getRelPointVel((0.0, 0.0, 0.0))
# TODO: Not 100% on this transpose
angular_vel = self.orientation.transpose().dot(self.angular_vel)
quaternion = self.body.getQuaternion()
translation = self.body.getPosition()
header = sub8_utils.make_header(frame='/world')
pose = geometry.Pose(
position=geometry.Point(*translation),
orientation=geometry.Quaternion(-quaternion[1], -quaternion[2], -quaternion[3], quaternion[0]),
)
twist = geometry.Twist(
linear=geometry.Vector3(*linear_vel),
angular=geometry.Vector3(*angular_vel)
)
odom_msg = Odometry(
header=header,
child_frame_id='/body',
pose=geometry.PoseWithCovariance(
pose=pose
),
twist=geometry.TwistWithCovariance(
twist=twist
)
)
self.truth_odom_pub.publish(odom_msg)
def make_wrench_stamped(force, torque, frame='/body'):
'''Make a WrenchStamped message without all the fuss
Frame defaults to body
'''
wrench = geometry_msgs.WrenchStamped(
header=make_header(frame),
wrench=geometry_msgs.Wrench(force=geometry_msgs.Vector3(*force),
torque=geometry_msgs.Vector3(*torque)))
return wrench
def send_wrench(self, force, torque):
'''
Specify wrench in Newtons, Newton-meters
(Are you supposed to say Newtons-meter? Newtons-Meters?)
'''
wrench_msg = geometry_msgs.WrenchStamped(
header=sub8_utils.make_header(),
wrench=geometry_msgs.Wrench(force=geometry_msgs.Vector3(*force),
torque=geometry_msgs.Vector3(*torque)))
self.wrench_pub.publish(wrench_msg)
def publishTagMarkers(tags, publisher):
"""Republishes the tag_markers topic"""
SHIFT = 0.05
h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map')
ca = std_msgs.ColorRGBA(r=1, a=1)
cb = std_msgs.ColorRGBA(g=1, a=1)
ct = std_msgs.ColorRGBA(b=1, a=1)
sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5)
sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25)
st = geometry_msgs.Vector3(x=1, y=1, z=1)
ma = visualization_msgs.MarkerArray()
ma.markers.append(
visualization_msgs.Marker(
header=h, id=-1, action=visualization_msgs.Marker.DELETEALL))
for i, t in enumerate(tags):
th = t['th_deg'] * math.pi / 180.
q = tf.transformations.quaternion_from_euler(0, 0, th)
q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t['x'], y=t['y']),
orientation=q),
scale=sa,
color=ca))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 1,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(
x=t['x'] + SHIFT * math.cos(th),
y=t['y'] + SHIFT * math.sin(th)),
orientation=q),
scale=sb,
color=cb))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 2,
type=visualization_msgs.Marker.TEXT_VIEW_FACING,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5),
orientation=q),
scale=st,
color=ct,
text=str(t['id'])))
publisher.publish(ma)
def to_message(self):
"""Convert current data to a trajectory point message."""
p_msg = TrajectoryPointMsg()
p_msg.header.stamp = rospy.Time(self.t)
p_msg.pose.position = geometry_msgs.Vector3(*self.p)
p_msg.pose.orientation = geometry_msgs.Quaternion(*self.q)
p_msg.velocity.linear = geometry_msgs.Vector3(*self.v)
p_msg.velocity.angular = geometry_msgs.Vector3(*self.w)
p_msg.acceleration.linear = geometry_msgs.Vector3(*self.a)
p_msg.acceleration.angular = geometry_msgs.Vector3(*self.alpha)
return p_msg
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Position error
e_pos_world = self.pos_des - p
e_pos_body = transf.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = numpy.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = transf.quaternion_multiply(
transf.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(x=v_linear[0],
y=v_linear[1],
z=v_linear[2])
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def adjust_position(self):
back_up_command = geometry_msgs.Twist(geometry_msgs.Vector3(-.5, 0, 0),
geometry_msgs.Vector3(0, 0, 0))
rotate_command = geometry_msgs.Twist(geometry_msgs.Vector3(0, 0, 0),
geometry_msgs.Vector3(0, 0, .6))
for _ in xrange(20):
# IPython.embed()
self._ros_cmd.publish(back_up_command)
self.rate.sleep()
for _ in xrange(30):
self._ros_cmd.publish(rotate_command)
self.rate.sleep()
self._ros_cmd.publish(geometry_msgs.Twist())
def SendNavigationMessage(self, request, context):
# TODO: This frame_id should be dynamically set from a config file.
nav_header = std_msgs.msg.Header(frame_id="fosenkaia_NED",
stamp=rospy.Time.from_sec(
request.timeStamp))
position = geomsgs.Point()
position.x = request.position.x
position.y = request.position.y
position.z = request.position.z
orientation = geomsgs.Quaternion()
orientation.x = request.orientation.x
orientation.y = request.orientation.y
orientation.z = request.orientation.z
orientation.w = request.orientation.w
pose_msg = geomsgs.PoseStamped(header=nav_header,
pose=geomsgs.Pose(
position=position,
orientation=orientation))
pose_pub.publish(pose_msg)
linear_vel = geomsgs.Vector3()
linear_vel.x = request.linearVelocity.x
linear_vel.y = request.linearVelocity.y
linear_vel.z = request.linearVelocity.z
angular_vel = geomsgs.Vector3()
angular_vel.x = request.angularVelocity.x
angular_vel.y = request.angularVelocity.y
angular_vel.z = request.angularVelocity.z
twist_msg = geomsgs.TwistStamped(header=nav_header,
twist=geomsgs.Twist(
linear=linear_vel,
angular=angular_vel))
twist_pub.publish(twist_msg)
transform = geomsgs.TransformStamped(header=nav_header,
child_frame_id="vessel_center",
transform=geomsgs.Transform(
translation=position,
rotation=orientation))
tf_pub.sendTransform(transform)
return navigation_pb2.NavigationResponse(success=True)
def to_geometry_msg(self) -> geometry_msgs.Vector3:
"""To ROS geometry_msg.
Returns:
This vector as a geometry_msgs/Vector3
"""
return geometry_msgs.Vector3(x=self.x, y=self.y, z=self.z)
def from_dict(self, dict_, root=None):
"""
This function creates a XForm tree from a dictionary object
:param dict_: The dictionary to create the XForm tree from
:type dict_: dict
:param root: The current root XForm of the tree
:type root: XForm
"""
if not root:
root = self
dict_ = dict_[dict_.keys()[0]]
for k, v in dict_.items():
if isinstance(v, dict):
child = XForm(root, name=k, ref_frame=dict_[k]['ref_frame'])
child.translation = gm_msg.Vector3(
x=dict_[k]['translation'][0],
y=dict_[k]['translation'][1],
z=dict_[k]['translation'][2])
child.rotation = gm_msg.Quaternion(x=dict_[k]['rotation'][0],
y=dict_[k]['rotation'][1],
z=dict_[k]['rotation'][2],
w=dict_[k]['rotation'][3])
self.from_dict(dict_[k], child)
def publish_dvl(self):
"""Publishes dvl sensor data - twist message, and array of 4 dvl velocities based off of ray orientations"""
correlation = -1
header = Header(
stamp=rospy.Time.now(),
frame_id='/world'
)
vel_dvl_body = self.body.vectorFromWorld(self.body.getRelPointVel(self.dvl_position))
velocity_measurements = []
for ray in self.dvl_ray_orientations:
velocity_measurements.append(VelocityMeasurement(
direction=geometry.Vector3(*ray),
velocity=np.dot(ray, vel_dvl_body),
correlation=correlation
))
dvl_msg = VelocityMeasurements(
header=header,
velocity_measurements=velocity_measurements
)
self.dvl_sensor_pub.publish(dvl_msg)
def draw_marker(self,
ps,
id=None,
type=Marker.CUBE,
ns='default_ns',
rgba=(0, 1, 0, 1),
scale=(.03, .03, .03),
text='',
duration=0):
"""
If markers aren't showing up, it's probably because the id's are being overwritten. Make sure to set
the ids and namespace.
"""
if id is None: id = self.get_unused_id()
marker = Marker(type=type, action=Marker.ADD)
marker.ns = ns
marker.header = ps.header
marker.pose = ps.pose
marker.scale = gm.Vector3(*scale)
marker.color = stdm.ColorRGBA(*rgba)
marker.id = id
marker.text = text
marker.lifetime = rospy.Duration(duration)
self.pub.publish(marker)
self.ids.add(id)
return MarkerHandle(marker, self.pub)
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
q = msg.orientation
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def make_pose_stamped(position, orientation, frame='/body'):
wrench = geometry_msgs.WrenchStamped(
header=make_header(frame),
pose=geometry_msgs.Pose(
force=geometry_msgs.Vector3(*position),
orientation=geometry_msgs.Quaternion(*orientation)))
return wrench
def to_message(self):
"""Convert current data to a trajectory point message.
> *Returns*
Trajectory point message as `uuv_control_msgs/TrajectoryPoint`
"""
p_msg = TrajectoryPointMsg()
# FIXME Sometimes the time t stored is NaN
p_msg.header.stamp = rospy.Time(self.t)
p_msg.pose.position = geometry_msgs.Vector3(*self.p)
p_msg.pose.orientation = geometry_msgs.Quaternion(*self.q)
p_msg.velocity.linear = geometry_msgs.Vector3(*self.v)
p_msg.velocity.angular = geometry_msgs.Vector3(*self.w)
p_msg.acceleration.linear = geometry_msgs.Vector3(*self.a)
p_msg.acceleration.angular = geometry_msgs.Vector3(*self.alpha)
return p_msg
def __transform_from_pose_2d(ps):
msg = gms.Transform()
msg.translation = gms.Vector3(x=ps.x, y=ps.y, z=__HEIGHT)
euler = (0.0, 0.0, ps.theta)
qa = t3de.euler2quat(*euler)
quat = gms.Quaternion(w=qa[0], x=qa[1], y=qa[2], z=qa[3])
msg.rotation = quat
return msg
def publish_action_twist(self):
v_w = self.domain.dynamics.setpoint_body.linearVelocity
w_w = self.domain.dynamics.setpoint_body.angularVelocity
a = self.domain.dynamics.grasp_body.angle
m = tf.transformations.rotation_matrix(-a, [0, 0, 1])[0:2, 0:2]
m *= (1.0 / self.domain.dynamics.simulation_scale)
v_g = np.dot(m, v_w)
v = geom_msg.Vector3(v_g[0], v_g[1], 0)
w = geom_msg.Vector3(0, 0, w_w)
ts = geom_msg.TwistStamped(
std_msg.Header(None, self.get_domain_sim_time(), "grasp"),
geom_msg.Twist(v, w))
self.action_twist_publisher.publish(ts)
def to_transform(pose, child_frame=None):
if isinstance(pose, geometry_msgs.Pose2D):
return geometry_msgs.Transform(
geometry_msgs.Vector3(pose.x, pose.y, 0.0),
quaternion_msg_from_yaw(pose.theta))
elif isinstance(pose, geometry_msgs.Pose):
return geometry_msgs.Transform(
geometry_msgs.Vector3(pose.position.x, pose.position.y,
pose.position.z), pose.orientation)
elif isinstance(pose, geometry_msgs.PoseStamped):
p = pose.pose
tf = geometry_msgs.Transform(
geometry_msgs.Vector3(p.position.x, p.position.y, p.position.z),
p.orientation)
return geometry_msgs.TransformStamped(pose.header, child_frame, tf)
raise rospy.ROSException(
"Input parameter pose is not a valid geometry_msgs pose object")
def cmd_vel_callback(msg):
global pose_lock, saved_pose, last_time, new_time
pose_lock.acquire()
ps = cp.copy(saved_pose)
pose_lock.release()
#if last_time is None:
# last_time = rp.get_rostime()
#else:
# last_time = cp.copy(new_time)
#new_time = rp.get_rostime()
#time_step = float(new_time.secs-last_time.secs) + 1e-9*np.float128(new_time.nsecs-last_time.nsecs)
#rp.logwarn(new_time)
#rp.logwarn(time_step)
if ps is None:
return
twist = gms.Twist()
translation = gms.Vector3()
# if ps.position[0] <= XLIM[0] and msg.linear[0] <= 0.0:
# twist.linear.x = 0.0
# translation.x = XLIM[0]
# elif ps.position[0] >= XLIM[1] and msg.linear[0] >= 0.0:
# twist.linear.x = 0.0
# translation.x = XLIM[1]
# else:
# twist.linear.x = msg.linear[0]
# translation.x = ps.position[0]
# if ps.position[1] <= YLIM[0] and msg.linear[1] <= 0.0:
# twist.linear.y = 0.0
# translation.y = YLIM[0]
# elif ps.position[1] >= YLIM[1] and msg.linear[1] >= 0.0:
# twist.linear.y = 0.0
# translation.y = YLIM[1]
# else:
# twist.linear.y = msg.linear[1]
# translation.y = ps.position[1]
twist.linear.x = msg.linear[0]
translation.x = ps.position[0]
twist.linear.y = msg.linear[1]
translation.y = ps.position[1]
twist.linear.z = 0.0
twist.angular.z = msg.angular
translation.z = ALTITUDE
rot_mat = np.eye(4)
rot_mat[0:2,0] = np.array(ps.orientation)
rot_mat[0:2,1] = utl.ccws_perp(ps.orientation)
quat = tft.quaternion_from_matrix(rot_mat)
rotation = gms.Quaternion()
rotation.x = quat[0]
rotation.y = quat[1]
rotation.z = quat[2]
rotation.w = quat[3]
transform = gms.Transform(translation, rotation)
cmd_traj = tms.MultiDOFJointTrajectory()
cmd_traj.points.append(tms.MultiDOFJointTrajectoryPoint())
cmd_traj.points[0].transforms.append(transform)
cmd_traj.points[0].velocities.append(twist)
cmd_traj_pub.publish(cmd_traj)
def publish_forcetorque(self):
ft_w = self.domain.forcetorque_measurement
f_w = ft_w[0:2]
# this is ft measured in the physics simulation frame.
# however, in real life, the sensor is mounted on the grasp frame.
# so project the force into the rotated frame.
a = self.domain.dynamics.grasp_body.angle
m = tf.transformations.rotation_matrix(-a, [0, 0, 1])[0:2, 0:2]
f_g = np.dot(m, f_w)
f = geom_msg.Vector3(f_g[0], f_g[1], 0)
t = geom_msg.Vector3(0, 0, ft_w[2])
ws = geom_msg.WrenchStamped(
std_msg.Header(None, self.get_domain_sim_time(), "grasp"),
geom_msg.Wrench(f, t))
self.forcetorque_publisher.publish(ws)
def list_to_vector3(list_):
"""
This function creates a Vector3 object from a position list
:param list_: A list of x, y, z attributes
:type list_: list
:return: A Vector3 object
:rtype: Vector3
"""
return gm_msg.Vector3(x=list_[0], y=list_[1], z=list_[2])
def __init__(self, f1, f2, listener, color='black'):
self.marker = VM.Marker()
self.marker.type = VM.Marker.LINE_LIST
self.marker.header.frame_id = "world"
self.marker.id = hash(f1+f2)%(2**16)
self.marker.scale = GM.Vector3(*(0.01,0.01,0.01))
self.set_color(color)
self.listener = listener
self.f1 = f1
self.f2 = f2
self.update_points()
def __init__(self, m):
self.marker = VM.Marker()
self.marker.type = VM.Marker.LINE_STRIP
scale = m.scale.x/10.0
self.marker.scale = GM.Vector3(*(scale, scale, scale))
self.marker.header.frame_id = m.header.frame_id
self.marker.id = hash(m.id + self.marker.type)%(2**16)
self.marker.color = m.color
self.marker.color.a = 1.0
self.point_list = []
self.max_size = rospy.get_param('path_len', NUMBER_MESSAGES)
self.update_path(m)
def publish_imu(self, dt):
"""Publishes imu sensor information - orientation, angular velocity, and linear acceleration"""
orientation_matrix = self.pose[:3, :3]
sigma = 0.01
noise = np.random.normal(0.0, sigma, 3)
# Work on a better way to get this
# We can't get this nicely from body.getForce()
g = self.body.vectorFromWorld(self.g)
# Get current velocity of IMU in body frame
cur_vel = np.array(self.body.vectorFromWorld(self.body.getRelPointVel(self.imu_position)))
linear_acc = orientation_matrix.dot(cur_vel - self.last_vel) / dt
linear_acc += g + (noise * dt)
# TODO: Fix frame
angular_vel = orientation_matrix.dot(self.body.getAngularVel()) + (noise * dt)
header = sub8_utils.make_header(frame='/body')
linear = geometry.Vector3(*linear_acc)
angular = geometry.Vector3(*angular_vel)
covariance = [sigma ** 2, 0., 0.,
0., sigma ** 2, 0.,
0., 0., sigma ** 2]
orientation_covariance = [-1, 0., 0.,
0., 0., 0.,
0., 0., 0.]
imu_msg = Imu(
header=header,
angular_velocity=angular,
angular_velocity_covariance=covariance,
linear_acceleration=linear,
linear_acceleration_covariance=covariance,
orientation_covariance=orientation_covariance
)
self.imu_sensor_pub.publish(imu_msg)
def create_marker(w):
global id_count
m = viz_msgs.Marker()
m.header.frame_id = w['frame_id']
m.ns = w['name']
m.id = id_count
m.action = viz_msgs.Marker.ADD
m.pose = create_geo_pose(w)
m.scale = geometry_msgs.Vector3(1.0,0.3,0.3)
m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)
id_count = id_count + 1
return m
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = msg.header.stamp.to_sec()
# Position error wrt. body frame
e_pos = trans.quaternion_matrix(q).transpose()[0:3,
0:3].dot(self.pos_des -
p)
# Error quaternion wrt body frame
e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q),
self.quat_des)
# Error angles
e_rot = numpy.array(trans.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(*v_linear)
cmd_vel.angular = geometry_msgs.Vector3(*v_angular)
self.pub_cmd_vel.publish(cmd_vel)
def publish_imu(self, dt):
"""Publishes imu sensor information - orientation, angular velocity, and linear acceleration"""
orientation_matrix = self.pose[:3, :3]
sigma = 0.01
noise = np.random.normal(0.0, sigma, 3)
linear_acc = orientation_matrix.dot(self.velocity - self.last_vel) / dt
linear_acc += noise
# TODO: Fix frame
angular_vel = orientation_matrix.dot(self.body.getAngularVel()) + noise
header = Header(
stamp=rospy.Time.now(),
frame_id='/world'
)
linear = geometry.Vector3(*linear_acc)
angular = geometry.Vector3(*angular_vel)
covariance = [sigma ** 2, 0., 0.,
0., sigma ** 2, 0.,
0., 0., sigma ** 2]
orientation_covariance = [-1, 0., 0.,
0., 0., 0.,
0., 0., 0.]
imu_msg = Imu(
header=header,
angular_velocity=angular,
angular_velocity_covariance=covariance,
linear_acceleration=linear,
linear_acceleration_covariance=covariance,
orientation_covariance=orientation_covariance
)
self.imu_sensor_pub.publish(imu_msg)
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
# Compute compute control output:
t = msg.header.stamp.to_sec()
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(*a_linear)
cmd_accel.angular = geometry_msgs.Vector3(*a_angular)
self.pub_cmd_accel.publish(cmd_accel)
