def rotate_and_publish(pose_stamped):
# Transform position
position = Vector3Stamped()
position.vector.x = pose_stamped.pose.position.x
position.vector.y = pose_stamped.pose.position.y
position.vector.z = pose_stamped.pose.position.z
position_frd = tf2_geometry_msgs.do_transform_vector3(
position, transform_frd)
# Transform rotation
quaternion_xyz = Vector3Stamped()
quaternion_xyz.vector.x = pose_stamped.pose.orientation.x
quaternion_xyz.vector.y = pose_stamped.pose.orientation.y
quaternion_xyz.vector.z = pose_stamped.pose.orientation.z
quaternion_xyz_frd = tf2_geometry_msgs.do_transform_vector3(
quaternion_xyz, transform_frd)
# Construct and publish FRD position
pose_stamped_frd = PoseStamped()
pose_stamped_frd.header = pose_stamped.header
pose_stamped_frd.pose.position.x = position_frd.vector.x
pose_stamped_frd.pose.position.y = position_frd.vector.y
pose_stamped_frd.pose.position.z = position_frd.vector.z
pose_stamped_frd.pose.orientation.x = quaternion_xyz_frd.vector.x
pose_stamped_frd.pose.orientation.y = quaternion_xyz_frd.vector.y
pose_stamped_frd.pose.orientation.z = quaternion_xyz_frd.vector.z
pose_stamped_frd.pose.orientation.w = pose_stamped.pose.orientation.w # WTF
vision_pose_pub.publish(pose_stamped_frd)
return pose_stamped_frd
def update_R_err(self):
e_R = np.dot(self.desired_R_.transpose(), self.R_) - \
np.dot(self.R_.transpose(), self.desired_R_)
self.e_R_ = self.vee(e_R) / 2.0
vec3_e_R = Vector3Stamped()
vec3_e_R.header.stamp = rospy.Time.now()
euler_des = self.rotationMatrixToEulerAngles(self.desired_R_)
vec3_e_R.vector = Vector3(euler_des[0], euler_des[1], euler_des[2])
self.publisher_euler_des.publish(vec3_e_R)
vec3_e_R = Vector3Stamped()
vec3_e_R.header.stamp = rospy.Time.now()
vec3_e_R.vector = Vector3(self.e_R_[0, 0], self.e_R_[1, 0],
self.e_R_[2, 0])
self.publisher_err_R.publish(vec3_e_R)
pub_vec3 = Vector3Stamped()
pub_vec3.header.stamp = rospy.Time.now()
pub_vec3.vector = Vector3(self.euler_quads[0, 0],
self.euler_quads[1, 0], self.euler_quads[2,
0])
self.publisher_eulers.publish(pub_vec3)
pub_vec3 = Vector3Stamped()
pub_vec3.header.stamp = rospy.Time.now()
pub_vec3.vector = Vector3(self.e_R_[0, 0], self.e_R_[1, 0],
self.e_R_[2, 0])
self.publisher_err_angles.publish(pub_vec3)
def do_transform_imu(imu_msg, transform):
"""
Transforms the given Imu message into the given target frame
:param imu_msg:
:type imu_msg: Imu
:param transform:
:type transform: TransformStamped
:return: Imu message transformed into the target frame
:rtype: Imu
"""
src_l = Vector3Stamped()
src_l.header = imu_msg.header
src_l.vector = imu_msg.linear_acceleration
src_a = Vector3Stamped()
src_a.header = imu_msg.header
src_a.vector = imu_msg.angular_velocity
src_o = QuaternionStamped()
src_o.header = imu_msg.header
src_o.quaternion = imu_msg.orientation
tgt_l = do_transform_vector3(src_l, transform)
tgt_a = do_transform_vector3(src_a, transform)
tgt_o = do_transform_quaternion(src_o, transform)
tgt_imu = copy.deepcopy(imu_msg)
tgt_imu.header.frame_id = transform.child_frame_id
tgt_imu.linear_acceleration = tgt_l
tgt_imu.angular_velocity = tgt_a
tgt_imu.orientation = tgt_o
return tgt_imu
def __init__(self):
self.p = Vector3Stamped()
self.p.header.frame_id = 'odom'
self.g = PointStamped()
self.g.point.x = -5
self.g.point.y = 5
self.g.point.z = 0
self.g.header.frame_id = 'odom'
self.a1 = Vector3Stamped()
self.a2 = Vector3Stamped()
self.a3 = Vector3Stamped()
self.a4 = Vector3Stamped()
self.a1.vector.x = 0.18
self.a1.vector.y = 0
self.a1.vector.z = 0
self.a1.header.frame_id = 'base_link'
self.a2.vector.x = 0
self.a2.vector.y = 0.18
self.a2.vector.z = 0
self.a2.header.frame_id = 'base_link'
self.a3.vector.x = -0.18
self.a3.vector.y = 0
self.a3.vector.z = 0
self.a3.header.frame_id = 'base_link'
self.a4.vector.x = 0
self.a4.vector.y = -0.18
self.a4.vector.z = 0
self.a4.header.frame_id = 'base_link'
self.listener = tf.TransformListener()
self.odom_msg = Odometry()
self.msg_rfid = EpcInfo()
self.base_EPC = rospy.get_param('RFIDrx/base1')
self.sub_odom = rospy.Subscriber('odom', Odometry, self.odometry_cb)
self.epc_publisher = rospy.Publisher('epcs', EpcInfo, queue_size=1)
def timer_callback(self):
now_msg = self.get_clock().now().to_msg()
info = {}
self.PS.write(b"A")
out = self.PS.readline()
try:
info = json.loads(out)
except:
pass
if len(info) > 0:
msg = Feet()
msg.header.stamp = now_msg
msg.right_front_force = info["RF"]["F"]
msg.right_front_valid = info["RF"]["S"] == 1
msg.right_back_force = info["RB"]["F"]
msg.right_back_valid = info["RB"]["S"] == 1
msg.left_front_force = info["LF"]["F"]
msg.left_front_valid = info["LF"]["S"] == 1
msg.left_back_force = info["LB"]["F"]
msg.left_back_valid = info["LB"]["S"] == 1
self.feetPublisher.publish(msg)
msg = Vector3Stamped()
msg.header.stamp = now_msg
msg.vector.x = info["ANG"]["X"]
msg.vector.y = info["ANG"]["Y"]
msg.vector.z = info["ANG"]["Z"]
self.posePublisher.publish(msg)
msg = Vector3Stamped()
msg.header.stamp = now_msg
msg.vector.x = info["GYR"]["X"]
msg.vector.y = info["GYR"]["Y"]
msg.vector.z = info["GYR"]["Z"]
self.twistPublisher.publish(msg)
def check_camera(name):
try:
img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
except rospy.ROSException:
failure('%s: no images (is the camera connected properly?)', name)
return
try:
camera_info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
except rospy.ROSException:
failure('%s: no calibration info', name)
return
if img.width != camera_info.width:
failure('%s: calibration width doesn\'t match image width (%d != %d)', name, camera_info.width, img.width)
if img.height != camera_info.height:
failure('%s: calibration height doesn\'t match image height (%d != %d))', name, camera_info.height, img.height)
try:
optical = Vector3Stamped()
optical.header.frame_id = img.header.frame_id
optical.vector.z = 1
cable = Vector3Stamped()
cable.header.frame_id = img.header.frame_id
cable.vector.y = 1
optical = describe_direction(tf_buffer.transform(optical, 'base_link').vector)
cable = describe_direction(tf_buffer.transform(cable, 'base_link').vector)
if not optical or not cable:
info('%s: custom camera orientation detected', name)
else:
info('camera is oriented %s, cable from camera goes %s', optical, cable)
except tf2_ros.TransformException:
failure('cannot transform from base_link to camera frame')
def execute(self):
"""this function gets called from pybullet ros main update loop"""
if not self.cmd_vel_msg:
return
# check if timestamp is recent
if (rospy.Time.now() -
rospy.Duration(0.5)) > self.received_cmd_vel_time:
return
# transform Twist from base_link to odom (pybullet allows to set vel only on world ref frame)
# NOTE: we would normally use tf for this, but there are issues currently between python 2 and 3 in ROS 1
lin_vec = Vector3Stamped()
lin_vec.header.frame_id = 'base_link'
lin_vec.vector.x = self.cmd_vel_msg.linear.x
lin_vec.vector.y = self.cmd_vel_msg.linear.y
lin_vec.vector.z = self.cmd_vel_msg.linear.z
ang_vec = Vector3Stamped()
ang_vec.header.frame_id = 'base_link'
ang_vec.vector.x = self.cmd_vel_msg.angular.x
ang_vec.vector.y = self.cmd_vel_msg.angular.y
ang_vec.vector.z = self.cmd_vel_msg.angular.z
lin_cmd_vel_in_odom = self.transformVector3('odom', lin_vec)
ang_cmd_vel_in_odom = self.transformVector3('odom', ang_vec)
# set vel directly on robot model
self.pb.resetBaseVelocity(self.robot, [
lin_cmd_vel_in_odom.vector.x, lin_cmd_vel_in_odom.vector.y,
lin_cmd_vel_in_odom.vector.z
], [
ang_cmd_vel_in_odom.vector.x, ang_cmd_vel_in_odom.vector.y,
ang_cmd_vel_in_odom.vector.z
])
def test_attach_existing_box_non_fixed(self, better_pose):
"""
:type zero_pose: Donbot
"""
pocky = u'box'
p = PoseStamped()
p.header.frame_id = u'refills_finger'
p.pose.position.y = -0.075
p.pose.orientation = Quaternion(0, 0, 0, 1)
better_pose.add_box(pocky, [0.05, 0.2, 0.03], p)
better_pose.attach_existing(pocky, frame_id=u'refills_finger')
tip_normal = Vector3Stamped()
tip_normal.header.frame_id = pocky
tip_normal.vector.y = 1
root_normal = Vector3Stamped()
root_normal.header.frame_id = u'base_footprint'
root_normal.vector.z = 1
better_pose.align_planes(pocky, tip_normal, u'base_footprint', root_normal)
pocky_goal = PoseStamped()
pocky_goal.header.frame_id = pocky
pocky_goal.pose.position.y = -.5
pocky_goal.pose.position.x = .3
pocky_goal.pose.position.z = -.2
pocky_goal.pose.orientation.w = 1
better_pose.allow_self_collision()
better_pose.set_translation_goal(pocky_goal, pocky, u'base_footprint')
better_pose.send_and_check_goal()
def __init__(self):
self.g = PointStamped()
self.listener = tf.TransformListener()
rospy.Subscriber('odom', Odometry, self.odometry_cb)
self.odom_msg = Odometry()
self.a1 = Vector3Stamped()
self.a2 = Vector3Stamped()
self.a3 = Vector3Stamped()
self.a4 = Vector3Stamped()
self.a1.vector.x = 0.18
self.a1.vector.y = 0
self.a1.vector.z = 0
self.a1.header.frame_id = 'base_link'
self.a2.vector.x = 0
self.a2.vector.y = 0.18
self.a2.vector.z = 0
self.a2.header.frame_id = 'base_link'
self.a3.vector.x = -0.18
self.a3.vector.y = 0
self.a3.vector.z = 0
self.a3.header.frame_id = 'base_link'
self.a4.vector.x = 0
self.a4.vector.y = -0.18
self.a4.vector.z = 0
self.a4.header.frame_id = 'base_link'
self.p = Vector3Stamped()
self.p.header.frame_id = 'odom'
self.yaw = [0, np.pi / 2, np.pi, 3 * np.pi / 2]
self.pose = PoseStamped()
self.pose.header.frame_id = 'base_link'
self.rate = rospy.Rate(1)
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.goal_pose = MoveBaseGoal()
def __init__(self):
self.euler_msg = Vector3Stamped()
self.euler_deg_msg = Vector3Stamped()
# Create subscribers and publishers.
self.pub_imu_euler = rospy.Publisher("imu_euler",
Vector3Stamped,
queue_size=1)
self.pub_imu_euler_deg = rospy.Publisher("imu_euler_deg",
Vector3Stamped,
queue_size=1)
self.pub_odom_euler = rospy.Publisher("odom_euler",
Vector3Stamped,
queue_size=1)
self.pub_odom_euler_deg = rospy.Publisher("odom_euler_deg",
Vector3Stamped,
queue_size=1)
self.pub_pose_euler = rospy.Publisher("pose_euler",
Vector3Stamped,
queue_size=1)
self.pub_pose_euler_deg = rospy.Publisher("pose_euler_deg",
Vector3Stamped,
queue_size=1)
sub_imu = rospy.Subscriber("imu", Imu, self.imu_callback)
sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback)
sub_pose = rospy.Subscriber("pose", Pose, self.pose_callback)
def simpleTrajectory2PPTrajectory(traj):
t = Trajectory()
for i in range(len(traj.t)):
p = Vector3Stamped()
v = Vector3Stamped()
a = Vector3Stamped()
j = Vector3Stamped()
p.header.stamp = rospy.Time(traj.t[i])
p.vector.x = traj.px[i]
p.vector.y = traj.py[i]
p.vector.z = traj.pz[i]
v.header.stamp = rospy.Time(traj.t[i])
v.vector.x = traj.vx[i]
v.vector.y = traj.vy[i]
v.vector.z = traj.vz[i]
a.header.stamp = rospy.Time(traj.t[i])
a.vector.x = traj.ax[i]
a.vector.y = traj.ay[i]
a.vector.z = traj.az[i]
j.header.stamp = rospy.Time(traj.t[i])
j.vector.x = traj.jx[i]
j.vector.y = traj.jy[i]
j.vector.z = traj.jz[i]
t.pos.append(p)
t.vel.append(v)
t.acc.append(a)
t.jerk.append(j)
return t
def createTrajMsg():
data = readFromFile('trajectory.txt')
traj = Trajectory()
z_default = 0.5
z_velDefault = 0
z_accDefault = 0
for data_line in data:
#timestamp
#time at data_line[0]
#traj.header.stamp.sec = data_line[0]
posLine = Vector3Stamped()
posLine.header.stamp = rospy.Time(data_line[0])
posLine.vector.x = data_line[1]
posLine.vector.y = data_line[2]
posLine.vector.z = z_default
traj.pos.append(posLine)
velLine = Vector3Stamped()
velLine.header = posLine.header
velLine.vector.x = data_line[3]
velLine.vector.y = data_line[4]
velLine.vector.z = z_velDefault
traj.vel.append(velLine)
accLine = Vector3Stamped()
accLine.header = posLine.header
accLine.vector.x = data_line[5]
accLine.vector.y = data_line[6]
accLine.vector.z = z_accDefault
traj.acc.append(accLine)
print traj
return traj
def _calc_accel(self):
"""
Calculate Acceleration Setpoint from Twist Setpoint PID
"""
if self._latest_vel_sp is None:
rospy.logwarn_throttle(
10.0, "{} | No vel setpoint.".format(rospy.get_name()))
return False
if self._latest_odom_fb is None:
rospy.logwarn_throttle(
10.0, "{} | No vel feedback.".format(rospy.get_name()))
return False
# Convert setpoint velocity into the correct frame
if self._latest_vel_sp.header.frame_id != self._latest_odom_fb.child_frame_id and len(
self._latest_vel_sp.header.frame_id) > 0:
if self._tf_buff.can_transform(self._latest_odom_fb.child_frame_id,
self._latest_vel_sp.header.frame_id,
rospy.Time.from_sec(0.0)):
cmd_vel = TwistStamped()
header = Header(self._latest_vel_sp.header.seq,
rospy.Time.from_sec(0.0),
self._latest_vel_sp.header.frame_id)
cmd_vel.twist.linear = self._tf_buff.transform(
Vector3Stamped(header, self._latest_vel_sp.twist.linear),
self._latest_odom_fb.child_frame_id,
rospy.Duration(5)).vector
cmd_vel.twist.angular = self._tf_buff.transform(
Vector3Stamped(header, self._latest_vel_sp.twist.angular),
self._latest_odom_fb.child_frame_id,
rospy.Duration(5)).vector
cmd_vel.header.stamp = rospy.Time.now()
else:
rospy.logwarn_throttle(
5, "{} | Cannot TF Velocity SP frame_id: {}".format(
rospy.get_name(), self._latest_vel_sp.header.frame_id))
return False
else:
cmd_vel = self._latest_vel_sp
clean_linear_vel = self._enforce_deadband(
self._latest_odom_fb.twist.twist.linear)
clean_angular_vel = self._enforce_deadband(
self._latest_odom_fb.twist.twist.angular)
vel_err = np.array([[cmd_vel.twist.linear.x - clean_linear_vel.x],
[cmd_vel.twist.linear.y - clean_linear_vel.y],
[cmd_vel.twist.linear.z - clean_linear_vel.z],
[
cmd_vel.twist.angular.z -
self._latest_odom_fb.twist.twist.angular.z
]],
dtype=float)
accel_sp_msg = AccelStamped()
accel_sp_msg.header = cmd_vel.header
accel_sp_msg.accel.linear.x, accel_sp_msg.accel.linear.y, accel_sp_msg.accel.linear.z, accel_sp_msg.accel.angular.z = self._vel_pids(
vel_err, self._latest_odom_fb.header.stamp.to_sec())
accel_sp_msg.accel.linear.y = 0 if not self._control_sway else accel_sp_msg.accel.linear.y
accel_sp_msg.accel.linear.z = 0 if not self._control_depth else accel_sp_msg.accel.linear.z
accel_sp_msg.accel.angular.z = 0 if not self._control_yaw else accel_sp_msg.accel.angular.z
self._latest_accel_sp = accel_sp_msg
return True
def __init__(self):
rospy.init_node("imu_orient")
# Subscribers and frames for Arduino data
rospy.Subscriber("imu/data_array",
ImuArray,
self.imu_array_callback,
queue_size=1)
rospy.Subscriber("imu/mag_array",
ImuMag,
self.imu_mag_callback,
queue_size=1)
rospy.Subscriber("imu/calibration",
ImuCalibration,
self.imu_calib_callback,
queue_size=1)
self.odom_frame_id = "odom"
self.base_frame_id = "base_link"
self.imu_frame_id = "imu_link"
# Messages and publishers for conversion
self.imu_msg = Imu()
self.imu_base_link_msg = Imu()
self.imu_msg.header.frame_id = self.imu_frame_id
self.imu_base_link_msg.header.frame_id = self.base_frame_id
self.imu_pub = rospy.Publisher("imu/data", Imu, queue_size=10)
self.imu_rpy_msg = Vector3Stamped()
self.imu_rpy_pub = rospy.Publisher("imu/rpy",
Vector3Stamped,
queue_size=10)
self.imu_mag_msg = Vector3Stamped()
self.imu_mag_pub = rospy.Publisher("imu/mag",
Vector3Stamped,
queue_size=10)
# Add transform to visualize orientation
#self.odom_broadcaster = TransformBroadcaster()
# Listener for base_link to imu_link transform to convert imu message into base_link frame
# Parameters for checking and storing calibration data
self.last_save_time = time()
self.calibration = ImuCalibration()
# Obtain the filepath for the IMU package (currently in 'sensors')
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('sensors')
self.imu_calib_filename = rospy.get_param(
"/imu/calib_filename",
self.package_path + "/config/imu_calibration.yaml")
# Procedure
# -save initial time of startup for when the calibration parameters were set
# -save calibration status and values when messages are received
# -during update function, check whether the calibration status is all 3's
# -if the status is all 3's then get the time since that last saved file
# -if time since last save has been one minute or more, save the new
# parameters in a yaml file and update the last save time
# Loop rate for repeated code
self.rate = rospy.Rate(30)
def test_place_in_shelf(self, shelf_setup):
"""
:type shelf_setup: Donbot
"""
box = u'box'
box_pose = PoseStamped()
box_pose.header.frame_id = u'map'
box_pose.pose.orientation.w = 1
box_pose.pose.position.y = -0.5
box_pose.pose.position.z = .1
shelf_setup.add_box(box, [0.05, 0.05, 0.2], box_pose)
grasp_pose = deepcopy(box_pose)
grasp_pose.pose.position.z += 0.05
grasp_pose.pose.orientation = Quaternion(*quaternion_from_matrix([[-1, 0, 0, 0],
[0, 0, 1, 0],
[0, 1, 0, 0],
[0, 0, 0, 1]]))
shelf_setup.allow_collision([CollisionEntry.ALL], box, [CollisionEntry.ALL])
# shelf_setup.allow_all_collisions()
shelf_setup.set_and_check_cart_goal(grasp_pose, u'refills_finger')
shelf_setup.attach_existing(box, u'refills_finger')
box_goal = PoseStamped()
box_goal.header.frame_id = u'map'
box_goal.pose.position.z = 1.12
box_goal.pose.position.y = -.9
grasp_pose.pose.orientation.w = 1
shelf_setup.set_translation_goal(box_goal, box)
tip_normal = Vector3Stamped()
tip_normal.header.frame_id = box
tip_normal.vector.z = 1
root_normal = Vector3Stamped()
root_normal.header.frame_id = u'base_footprint'
root_normal.vector.z = 1
shelf_setup.align_planes(box, tip_normal, u'base_footprint', root_normal)
shelf_setup.send_and_check_goal()
box_goal = PoseStamped()
box_goal.header.frame_id = box
box_goal.pose.position.y = -0.2
grasp_pose.pose.orientation.w = 1
shelf_setup.set_translation_goal(box_goal, box)
tip_normal = Vector3Stamped()
tip_normal.header.frame_id = box
tip_normal.vector.z = 1
root_normal = Vector3Stamped()
root_normal.header.frame_id = u'base_footprint'
root_normal.vector.z = 1
shelf_setup.align_planes(box, tip_normal, u'base_footprint', root_normal)
shelf_setup.send_and_check_goal()
def do_transform_wrench(wrench, transform):
force = Vector3Stamped()
torque = Vector3Stamped()
force.vector = wrench.wrench.force
torque.vector = wrench.wrench.torque
res = WrenchStamped()
res.wrench.force = do_transform_vector3(force, transform).vector
res.wrench.torque = do_transform_vector3(torque, transform).vector
res.header = transform.header
return res
def twist_in_frame(tf_buffer, twist_s, frame_id):
t = get_transform(tf_buffer, frame_id, twist_s.header.frame_id)
if not t:
return None
h = Header(frame_id=frame_id, stamp=twist_s.header.stamp)
vs_l = Vector3Stamped(header=h, vector=twist_s.twist.linear)
vs_a = Vector3Stamped(header=h, vector=twist_s.twist.angular)
msg = TwistStamped(header=h)
msg.twist.linear = tf2_geometry_msgs.do_transform_vector3(vs_l, t).vector
msg.twist.angular = tf2_geometry_msgs.do_transform_vector3(vs_a, t).vector
return msg
def transform_twist(listener, base_frame, target_frame, twist):
lin = Vector3Stamped()
ang = Vector3Stamped()
lin.vector = twist.linear
ang.vector = twist.angular
lin.header.frame_id = base_frame
ang.header.frame_id = base_frame
twist_tf = Twist()
twist_tf.linear = listener.transformVector3(target_frame, lin).vector
twist_tf.angular = listener.transformVector3(target_frame, ang).vector
return twist_tf
def callback(self, qtc, ppl):
vels = []
try:
t = self.tf.getLatestCommonTime("base_link", ppl.header.frame_id)
vs = Vector3Stamped(header=ppl.header)
vs.header.stamp = t
for v in ppl.velocities:
vs.vector = v
vels.append(self.tf.transformVector3("base_link", vs).vector)
except Exception as e:
rospy.logwarn(e)
return
data_buffer = {
e.uuid: {
"qtc": json.loads(e.qtc_serialised),
"angle": ppl.angles[ppl.uuids.index(e.uuid)],
"velocity": vels[ppl.uuids.index(e.uuid)]
}
for e in qtc.qtc
}
try:
element = data_buffer[ppl.uuids[ppl.distances.index(
ppl.min_distance)]] # Only taking the closest person for now
self.publish_closest_person_marker(
ppl.poses[ppl.distances.index(ppl.min_distance)],
ppl.header.frame_id)
except KeyError:
return
qtc = element["qtc"].split(',')
self.cc.publish(angle=element["angle"],
qtc_symbol=','.join([qtc[0]] if len(qtc) ==
2 else [qtc[0], qtc[2]]),
velocity=element["velocity"])
def getState(self, posData):
# odomEstimate = self.getOdomEstimate()
# velxEstimate = odomEstimate.twist.twist.linear.x
# velyEstimate = odomEstimate.twist.twist.linear.y
pose = posData.pose[1]
position = pose.position
orientation = pose.orientation
t = tf.TransformerROS(True, rospy.Duration(10.0))
m = TransformStamped()
m.header.frame_id = 'world'
m.child_frame_id = 'base_link'
m.transform.translation.x = position.x
m.transform.translation.y = position.y
m.transform.translation.z = position.z
m.transform.rotation.x = orientation.x
m.transform.rotation.y = orientation.y
m.transform.rotation.z = orientation.z
m.transform.rotation.w = orientation.w
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
t.setTransform(m)
velx = posData.twist[1].linear.x
vely = posData.twist[1].linear.y
velz = posData.twist[1].linear.z
# Transform to body frame velocities
velVector = Vector3Stamped()
velVector.vector.x = velx
velVector.vector.y = vely
velVector.vector.z = velz
velVector.header.frame_id = "world"
# velVectorTransformed = self.tl.transformVector3("base_link", velVector)
velVectorTransformed = t.transformVector3("base_link", velVector)
velxBody = velVectorTransformed.vector.x
velyBody = velVectorTransformed.vector.y
velzBody = velVectorTransformed.vector.z
carTangentialSpeed = math.sqrt(velx ** 2 + vely ** 2)
carAngularVel = posData.twist[1].angular.z
stateInfo = {
"x": position.x, "y": position.y,
"i": orientation.x, "j": orientation.y,
"k": orientation.z, "w": orientation.w,
"xdot": velx, "ydot": vely,
"thetadot": carAngularVel,
"s": carTangentialSpeed,
"xdotbodyframe": velxBody, "ydotbodyframe": velyBody
}
state = []
for key in self.state_info:
state.append(stateInfo[key])
#rewardState = [stateInfo["thetadot"], stateInfo["xdotbodyframe"], stateInfo["ydotbodyframe"]]
return np.array(state)
def write_sub(self, vector, parameter):
output = Vector3Stamped()
output.header.frame_id = parameter
output.vector.x = vector[0]
output.vector.y = vector[1]
output.vector.z = vector[2]
return output
def set_accel(self):
plc = self.ros_data["plc"]
rx, ry, rz, r, p, y = self.parse_local_position("e")
local_pos = np.array([rx, ry, rz])
self.path_rec.append(local_pos.copy())
loc_goal = self.goal - local_pos
ax, ay, az = self.parse_linear_acc()
vx, vy, vz, _, _, _ = self.parse_velocity()
state = np.array([rx, ry, rz, vx, vy, vz, ax, ay, az])
accel, self.loc_goal_old, self.f_angle = control_method.start(
plc, state, loc_goal, r, p, y, self.loc_goal_old, self.f_angle,
self.path_rec)
acc = Vector3Stamped()
acc.header = Header()
acc.header.frame_id = "map"
acc.header.stamp = rospy.Time.now()
acc.vector.x = accel[0]
acc.vector.y = accel[1]
acc.vector.z = -accel[2] - self.g
# acc.accel.angular.x = 0
# acc.accel.angular.y = 0
# acc.accel.angular.z = 0
# rospy.Publisher(
# 'mavros/setpoint_accel/send_force', True, queue_size=10)
self.pos_setacc_pub.publish(acc)
def my_callback(event):
global mov_x
global mov_y
global abs_x
global abs_y
vec = Vector3Stamped()
vec.header.stamp = rospy.Time.now()
vec.vector = Vector3(x=mov_x*scaling, y=mov_y*scaling, z=0)
pub.publish(vec)
print(vec)
#odom = Odometry()
#odom.header.stamp = rospy.Time.now()
#try:
# transf = tfBuffer.lookup_transform(BASE_FRAME, 'mouse_front', rospy.Time())
# vec = tf2_geometry_msgs.do_transform_vector3(vec, transf)
# abs_x += vec.vector.x
# abs_y += vec.vector.y
#except Exception as ex:
# print(ex)
# abs_x = abs_x + mov_x*scaling
# abs_y = abs_y + mov_y*scaling
#odom.header.frame_id = "mouse_front"
# since all odometry is 6DOF we'll need a quaternion created from yaw
#odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
#odom.pose.pose = Pose(Point(abs_x, abs_y, 0.), Quaternion(*odom_quat))
#odom.child_frame_id = "base_link"
#odom.twist.twist = Twist(Vector3(mov_x, mov_y, 0), Vector3(0, 0, 0))
#odom_pub.publish(odom)
mov_x = 0
mov_y = 0
def create_vector3_stamped(vector, frame_id='base_link'):
m = Vector3Stamped()
m.header.frame_id = frame_id
m.header.stamp = rospy.Time()
#m.header.stamp = rospy.get_rostime()
m.vector = Vector3(*vector)
return m
def get_lines(self, fmt):
t = Time()
t.secs = 0
# MESSAGE_TO_TUM_SHORT
line = ROSMsg2CSVLine.to(fmt, Point(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
line = ROSMsg2CSVLine.to(fmt, PointStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED)
line = ROSMsg2CSVLine.to(fmt, Vector3(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
line = ROSMsg2CSVLine.to(fmt, Vector3Stamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED)
line = ROSMsg2CSVLine.to(
fmt, PoseWithCovarianceStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED)
line = ROSMsg2CSVLine.to(
fmt, PoseWithCovariance(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE)
line = ROSMsg2CSVLine.to(fmt, PoseStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED)
line = ROSMsg2CSVLine.to(fmt, Pose(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSE)
line = ROSMsg2CSVLine.to(fmt, Quaternion(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNION)
line = ROSMsg2CSVLine.to(
fmt, QuaternionStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED)
line = ROSMsg2CSVLine.to(fmt, Transform(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM)
line = ROSMsg2CSVLine.to(
fmt, TransformStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED)
return line
def pub_rew(self, rx, ry=0, rz=0):
reward = Vector3Stamped()
reward.vector.x = rx
reward.vector.y = ry
reward.vector.z = rz
reward.header.stamp = rospy.Time.now()
self._ros_reward_pub.publish(reward)
def __init__(self):
# Create a publisher for acceleration data
self.pub = rospy.Publisher('/master/control/error',
Vector3Stamped,
queue_size=10)
# Create variable for use
self.error_vector = Vector3Stamped()
# The offset of the quad from were it wants to be
self.setpoint_x = 0 # Creating these values here in the master controller
self.setpoint_y = 0
self.setpoint_z = 0
self.measured_value_x = 0 # Subscribe to these, x and y are the roomba location
self.measured_value_y = 0
self.measured_value_z = 0 # Subscribe to this, the altitude
self.rate = rospy.Rate(10) # 10hz
self.state = 1 # starts in the take off state
self.at_target_location = 0 # 1. need to produce this
self.look_for_roomba = 0
# self.obstacle_in_the_way = 0 # 4. Subscribe to this?
self.need_to_land = 0 # 5. calculate here?
self.main() # Runs the main def
def __init__(self):
self.listener = TransformListener()
# Subscribe to the ROS topic that contains the grasps.
self.sub = rospy.Subscriber('/detect_grasps/clustered_grasps',
GraspConfigList, self.grasp_callback)
self.pub = rospy.Publisher('/object_pose', Pose, queue_size=1)
self.grasp_acc = []
self.tempPoint = PointStamped()
#self.msg = PointStamped()
self.transformedPoint = PointStamped()
self.orient = Vector3Stamped()
## initiate a RobotCommander object. This object is an interface to the
## robot as a whole
self.robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
self.scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints.
self.group = moveit_commander.MoveGroupCommander("manipulator")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
def __init__(self, god_map, tip, goal_point, root=None, pointing_axis=None, weight=HIGH_WEIGHT):
super(Pointing, self).__init__(god_map)
if root is None:
self.root = self.get_robot().get_root()
else:
self.root = root
self.tip = tip
goal_point = convert_dictionary_to_ros_message(u'geometry_msgs/PointStamped', goal_point)
goal_point = transform_point(self.root, goal_point)
if pointing_axis is not None:
pointing_axis = convert_dictionary_to_ros_message(u'geometry_msgs/Vector3Stamped', pointing_axis)
pointing_axis = transform_vector(self.tip, pointing_axis)
else:
pointing_axis = Vector3Stamped()
pointing_axis.header.frame_id = self.tip
pointing_axis.vector.z = 1
tmp = np.array([pointing_axis.vector.x, pointing_axis.vector.y, pointing_axis.vector.z])
tmp = tmp / np.linalg.norm(tmp) # TODO possible /0
pointing_axis.vector = Vector3(*tmp)
params = {self.goal_point: goal_point,
self.pointing_axis: pointing_axis,
self.weight: weight}
self.save_params_on_god_map(params)
def __init__(self):
# Create a publisher for acceleration data
self.pub = rospy.Publisher('/roomba/location_meters',
Vector3Stamped,
queue_size=10)
self.rate = rospy.Rate(10) # 10hz
# For finding radius
self.a = 391.3
self.b = -0.9371
self.radius = 0
# Altitude
self.alt = 0.8128 # 32 inches ~ as high as the countertop # NEEDS TO CHANGE FROM HARD CODE TO SUBSCRIPTION
self.location_new = Vector3Stamped()
self.location_old = Vector3Stamped()
self.cap = cv2.VideoCapture(0)
self.loop_search()
