def test_cb(self, msg):
self.hello()
self.forward_DH()
br = TransformBroadcaster()
ls = tf.TransformListener()
tar_2_msg = g_msg.TransformStamped()
tar_3_msg = g_msg.TransformStamped()
pos, ros = None, None
try:
ls.waitForTransform("tool_target", "iiwa_base_link", rospy.Time(0),
rospy.Duration(4.0))
(pos, rot) = ls.lookupTransform('iiwa_base_link', 'tool_target',
rospy.Time())
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("can't get iiwa_base_link <- tool_target transform")
return -1
pos[0] += 0.4
#br.sendTransform( pos, rot, rospy.Time.now(), 'tool_target_2', 'iiwa_base_link' )
tar_2_msg.header.stamp = rospy.Time.now()
tar_2_msg.header.frame_id = 'iiwa_base_link'
tar_2_msg.child_frame_id = 'tool_target_2'
tar_2_msg.transform.translation.x = pos[0]
tar_2_msg.transform.translation.y = pos[1]
tar_2_msg.transform.translation.z = pos[2]
tar_2_msg.transform.rotation.x = rot[0]
tar_2_msg.transform.rotation.y = rot[1]
tar_2_msg.transform.rotation.z = rot[2]
tar_2_msg.transform.rotation.w = rot[3]
br.sendTransformMessage(tar_2_msg)
def update(self):
"""
Retrieves the transform from the blackboard, stamps it
and subsequently broadcasts it.
Raises:
TypeError: if the blackboard variable is of the wrong type.
"""
try:
transform = self.blackboard.get(self.variable_name)
except KeyError:
self.feedback_message = "no transform to broadcast"
return py_trees.common.Status.FAILURE
if transform is None:
self.feedback_message = "no transform to broadcast"
return py_trees.common.Status.FAILURE
if type(transform) != geometry_msgs.Transform:
raise TypeError(
"'{}' is not of type geometry_msgs/Transform".format(
self.variable_name))
self.feedback_message = "transform sent"
transform_stamped = geometry_msgs.TransformStamped()
transform_stamped.header.stamp = rclpy.clock.Clock().now().to_msg()
transform_stamped.header.frame_id = self.source_frame
transform_stamped.child_frame_id = self.target_frame
transform_stamped.transform = transform
# print(console.green + "Send: {}".format(transform_stamped) + console.reset)
self.broadcaster.sendTransform(transform_stamped)
return py_trees.common.Status.SUCCESS
def start_tf_msg(_start):
_msg = geom.TransformStamped()
_msg.header.stamp = rospy.Time.now()
_msg.header.frame_id = "map"
_msg.child_frame_id = "base_link"
_msg.transform.translation.x = _start.x
_msg.transform.translation.y = _start.y
_msg.transform.translation.z = 0
_msg.transform.rotation = geom.Quaternion(0, 0, 0, 1)
return _msg
def __init__(self):
rospy.init_node('costBasdMovement')
robberName = "roy"
self.robLoc = geo_msgs.TransformStamped()
rospy.Subscriber("/" + robberName + "/base_footprint", geo_msgs.TransformStamped, self.getRobberLocation)
print(self.robLoc) #doesnt work???
#createGrid(self.robLoc)
mapPub()
def callback(data, args):
broadcaster = tf2_ros.TransformBroadcaster()
tf1 = geo_msg.TransformStamped()
tf1.header.stamp = rospy.Time.now()
tf1.header.frame_id = args[0]
tf1.child_frame_id = args[1]
tf1.transform.translation = data.pose.pose.position
tf1.transform.rotation = data.pose.pose.orientation
broadcaster.sendTransform(tf1)
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 publish_transform(transform, reference_frame, name, seconds=1):
"""
Publish a Transform for debugging purposes.
transform -> A geometry_msgs/Transform to be published
reference_frame -> A string defining the reference frame of the transform
seconds -> An int32 that defines the duration the transform will be broadcast for
"""
# Create a stamped_transform and store the transform in it
st = gmsg.TransformStamped()
st.transform = transform
st.header.frame_id = reference_frame
st.child_frame_id = name
# Call the publish_stamped_transform function
publish_stamped_transform(st, seconds)
def send_transform(self):
t = geometry_msgs.TransformStamped()
t.header.stamp = rclpy.clock.Clock().now().to_msg()
t.header.frame_id = source_frame()
t.child_frame_id = target_frame()
t.transform.translation.x = self.x
t.transform.translation.y = 0.0
t.transform.translation.z = 0.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0
print(console.green + "Broadcast: {}".format(self.x) + console.reset)
self.broadcaster.sendTransform(t)
self.x += 0.1
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 __init__(self):
# state variables
self.last_stand = RapiroOdom.BOTH
self.last_r_yaw = math.pi / 2.
self.last_l_yaw = math.pi / 2.
self.last_time = rospy.Time.now()
# all TF stuff
self.tf_pub = tf2.TransformBroadcaster()
self.tf_msg = geometry_msgs.TransformStamped()
self.tf_msg.header.frame_id = 'odom'
self.tf_msg.child_frame_id = 'base_link'
self.tf_msg.transform.rotation.w = 1.
# all Odometry stuff
self.odom_pub = rospy.Publisher("odom", nav_msgs.Odometry, queue_size=10)
self.odom_msg = nav_msgs.Odometry()
self.odom_msg.header.frame_id = 'odom'
self.odom_msg.child_frame_id = 'base_link'
self.odom_msg.pose.pose.orientation.w = 1.
def handleZumoPos(velMsg):
global x
global y
global th
br = tfRos.TransformBroadcaster()
t = geoMsg.TransformStamped()
print("x: ", x)
print("y: ", y)
print("t: ", th)
vx = 0
if (velMsg.linear.x == 1.0):
vx = 0.1
elif (velMsg.linear.x == 1.0):
vx = -0.1
deltaX = vx * math.cos(th)
deltaY = vx * math.sin(th)
deltaTh = 0
x += deltaX
y += deltaY
th += deltaTh
t.header.stamp = ros.Time.now()
t.header.frame_id = "world"
t.child_frame_id = "zumo"
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = 0.0
q = tfConversions.transformations.quaternion_from_euler(0, 0, th)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
br.sendTransform(t)
def _load_transform(self):
if rospy.get_param("~dummy", False):
child = rospy.get_param("~camera_frame")
if rospy.get_param("~eye_on_hand", False):
parent = rospy.get_param("~robot_effector_frame")
T = CalibrationPublisher.Tec
else:
parent = rospy.get_param("~robot_base_frame")
T = CalibrationPublisher.Tbc
else:
parent = rospy.get_param("~parent")
child = rospy.get_param("~child")
T = rospy.get_param("~transform")
transform = gmsg.TransformStamped()
transform.header.frame_id = parent
transform.child_frame_id = child
transform.transform \
= gmsg.Transform(gmsg.Vector3(T["x"], T["y"], T["z"]),
gmsg.Quaternion(T["qx"], T["qy"],
T["qz"], T["qw"]))
return transform
def publish_pose_as_transform(pose, reference_frame, name, seconds=1):
"""
Publish a pose as a transform so that it is visualised in rviz.
pose -> A geometry_msgs.msg/Pose to be converted into a transform and published
reference_frame -> A string defining the reference_frame of the pose
name -> A string defining the child frame of the transform
seconds -> An int32 that defines the duration the transform will be broadcast for
"""
# Create a broadcast node and a stamped transform to broadcast
t = gmsg.TransformStamped()
# Prepare broadcast message
t.header.frame_id = reference_frame
t.child_frame_id = name
# Copy in pose values to transform
t.transform.translation = pose.position
t.transform.rotation = pose.orientation
# Call the publish_stamped_transform function
publish_stamped_transform(t, seconds)
def publishWalls():
cloudPoints = []
if (wallLeft > 0):
cloudPoints.append([0.5, -13.0 / wallLeft, 0.5])
#print("Object on Left");
if (wallRight > 0):
cloudPoints.append([0.5, 13.0 / wallRight, 0.5])
#print("Object on Right");
if (wallFrontLeft > 0):
cloudPoints.append([13.0 / wallFrontLeft, 0.5, 0.5])
#print("Object at Front");
if (wallFrontRight > 0):
cloudPoints.append([13.0 / wallFrontRight, -0.5, 0.5])
#print("Object at Front");
header = stdMsg.Header()
header.stamp = ros.Time.now()
header.frame_id = 'world'
myPointCloud = pcl2.create_cloud_xyz32(header, cloudPoints)
t = geoMsg.TransformStamped()
t.header.stamp = ros.Time.now()
t.header.frame_id = 'world'
t.child_frame_id = 'zumo'
t.transform.translation.x = transX
t.transform.translation.y = transY
t.transform.translation.z = transZ
t.transform.rotation.x = rotX
t.transform.rotation.y = rotY
t.transform.rotation.z = rotZ
t.transform.rotation.w = rotW
cloudOut = doTransformCloud(myPointCloud, t)
pclPub.publish(cloudOut)
def publishTransform(): global x; global y; global th; br = tfRos.TransformBroadcaster(); t = geoMsgs.TransformStamped(); t.header.stamp = ros.Time.now(); t.header.frame_id = "odom"; t.child_frame_id = "base_link"; t.transform.translation.x = x; t.transform.translation.y = y; t.transform.translation.z = 0.0; q = tfConversions.transformations.quaternion_from_euler(0, 0, th); t.transform.rotation.x = q[0]; t.transform.rotation.y = q[1]; t.transform.rotation.z = q[2]; t.transform.rotation.w = q[3]; br.sendTransform(t);
def main():
# Parse arguments
parser = argparse.ArgumentParser(
description=
'Set frames being broadcast by a multi_static_transform_publisher')
parser.add_argument(
'--node-name',
metavar=('node_name'),
default=['multi_tf_pub'],
type=str,
nargs=1,
help=
'The name of the multi publisher that should publish this transform. Default is \'multi_tf_pub\''
)
parser.add_argument('--xyz',
metavar=('x', 'y', 'z'),
default=[0.0, 0.0, 0.0],
type=float,
nargs=3,
help='Position in x, y, z)')
group = parser.add_mutually_exclusive_group()
group.add_argument('--quat',
metavar=('qx', 'qy', 'qz', 'qw'),
default=[0.0, 0.0, 0.0, 1.0],
type=float,
nargs=4,
help='Orientation in quaternion')
group.add_argument('--aa',
metavar=('x', 'y', 'z', 't'),
type=float,
nargs=4,
help='Orientation in axis/angle x, y, z, theta')
group.add_argument('--ypr',
metavar=('yaw', 'pitch', 'roll'),
type=float,
nargs=3,
help='Orientation in yaw, pitch, roll')
parser.add_argument(
'parent_frame_id',
metavar=('frame_id'),
type=str,
nargs=1,
help='The frame_id of the frame in which new new frame is defined.')
parser.add_argument('child_frame_id',
metavar=('child_frame_id'),
type=str,
nargs=1,
help='The frame_id of the new frame.')
parser.add_argument('period',
metavar=('period'),
type=float,
nargs=1,
help='Publish period in ms.')
myargv = rospy.myargv(argv=sys.argv)
args = parser.parse_args(args=myargv[1:])
rospy.init_node('multi_static')
# Check if other rotation format is used
if args.ypr:
# Transform YPR to quat
rotation = PyKDL.Rotation.RPY(args.ypr[2], args.ypr[1], args.ypr[0])
args.quat = rotation.GetQuaternion()
elif args.aa:
# Transform axis angle to quat
axis = PyKDL.Vector(args.aa[0], args.aa[1], args.aa[2])
axis.Normalize()
rotation = PyKDL.Rotation.Rot(axis, args.aa[3])
args.quat = rotation.GetQuaternion()
# Construct transform message
tform = geometry_msgs.TransformStamped()
tform.header.frame_id = args.parent_frame_id[0]
tform.header.stamp = rospy.Time(0)
tform.child_frame_id = args.child_frame_id[0]
tform.transform.translation.x = args.xyz[0]
tform.transform.translation.y = args.xyz[1]
tform.transform.translation.z = args.xyz[2]
tform.transform.rotation.x = args.quat[0]
tform.transform.rotation.y = args.quat[1]
tform.transform.rotation.z = args.quat[2]
tform.transform.rotation.w = args.quat[3]
static_tform = lcsr_tf_tools.StaticTransform()
static_tform.header.stamp = rospy.Time.now()
static_tform.transform = tform
static_tform.publish_period = rospy.Duration(1E-3 * args.period[0])
# Publish the transform
rospy.loginfo("Registering static transform %s --> %s\n%s" %
(tform.header.frame_id, tform.child_frame_id, str(tform)))
set_pub = None
# Get a tf_listener to make sure the frame gets published
listener = tf.TransformListener(True, rospy.Duration(10.0))
# Wait for the frame to be published
while listener and not rospy.is_shutdown():
static_tform.header.stamp = rospy.Time.now()
set_pub = rospy.Publisher(args.node_name[0] + '/set_frame',
lcsr_tf_tools.StaticTransform,
latch=True)
set_pub.publish(static_tform)
try:
now = rospy.Time(0)
listener.waitForTransform(tform.header.frame_id,
tform.child_frame_id, now,
rospy.Duration(1.0))
pose = listener.lookupTransform(tform.header.frame_id,
tform.child_frame_id, now)
rospy.loginfo("Registered static transform %s --> %s\n%s: %s" %
(tform.header.frame_id, tform.child_frame_id,
str(tform), str(pose)))
listener = None
except tf.Exception as ex:
rospy.logwarn(
"Multi static transform %s --> %s not being published yet: %s"
% (tform.header.frame_id, tform.child_frame_id, str(ex)))
# Wait for sigint
rospy.spin()
def sendTransform(data):
# initialise transform msg
t = g.TransformStamped()
# Populate transform data
t.header.stamp = rospy.Time.now()
t.header.frame_id = 'map'
t.child_frame_id = 'laser'
# set transform translation values
t.transform.translation.x = data.x
t.transform.translation.y = data.y
t.transform.translation.z = 0
# set transform rotation values
q = tf.transformations.quaternion_from_euler(0, 0, 0)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
# rospy.logout(data)
# Send transform data
tfB = tf2_ros.TransformBroadcaster()
tfB.sendTransform(t)
# pose = g.PoseStamped()
# setpoint = geometry_msgs.msg.TransformStamped()
# tf_buffer = tf2_ros.Buffer(rospy.Duration(10.0)) #tf buffer length
# tf_listener = tf2_ros.TransformListener(tf_buffer)
# rate = rospy.Rate(20) # 20hz
# local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local', geometry_msgs.msg.PoseStamped, queue_size=10)
# pose.pose.position.x = 0
# pose.pose.position.y = 0
# pose.pose.position.z = 1
# local_pos_pub.publish(pose)
# set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
# arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
# br= tf2_ros.TransformBroadcaster()
# setpoint.header.stamp = rospy.Time.now()
# setpoint.header.frame_id = "map"
# setpoint.child_frame_id = "setpoint"
# setpoint.transform.translation.x = 0.0
# setpoint.transform.translation.y = 0.0
# setpoint.transform.translation.z = 1.0
# q = tf.transformations.quaternion_from_euler(0, 0, 0)
# setpoint.transform.rotation.x = q[0]
# setpoint.transform.rotation.y = q[1]
# setpoint.transform.rotation.z = q[2]
# setpoint.transform.rotation.w = q[3]
# for i in range(100):
# #local_pos_pub.publish(pose)
# br.sendTransform(setpoint)
# rate.sleep()
# set_mode(0,"OFFBOARD" )
# arm(True)
# while not rospy.is_shutdown():
# #local_pos_pub.publish(pose)
# #br.sendTransform(setpoint)
# rate.sleep()
def roi_callback(self, msg):
rospy.loginfo(
rospy.get_caller_id() +
" Mesh filepaths received. Starting Camera Placement Optimization."
)
# launch optimization from here
best_placements = optimize(
seg_env_prototype=make_path_absolute(msg.surf_path_prototype),
target_prototype=make_path_absolute(msg.target_path_prototype),
cluster_env_path=make_path_absolute(msg.clustered_env_path),
full_env_path=make_path_absolute(msg.full_env_path),
enable_user_confirmation=True)
rospy.loginfo(" Publishing Optimal Placements...")
count = 0
for bp in best_placements:
rospy.loginfo(" Publishing Camera " + str(count))
pch_msg = pchMsg()
state_msg = ModelState()
static_transform = geoMsg.TransformStamped()
pos_msgs = [
pch_msg.pose.position, state_msg.pose.position,
static_transform.transform.translation
]
rot_msgs = [
pch_msg.pose.orientation, state_msg.pose.orientation,
static_transform.transform.rotation
]
h = Header()
h.stamp = rospy.Time.now()
q = eul_to_qt(bp.pose[3:])
cam_name = "iris_" + str(count + 1)
frame_name = "iris_" + str(count +
1) + "/camera__optical_center_link"
for i in range(len(pos_msgs)):
pos_msgs[i].x = bp.pose[0]
pos_msgs[i].y = bp.pose[1]
pos_msgs[i].z = bp.pose[2]
rot_msgs[i].x = q[0]
rot_msgs[i].y = q[1]
rot_msgs[i].z = q[2]
rot_msgs[i].w = q[3]
pch_msg.header = h
pch_msg.camera_id = bp.camera_id
# pch_msg.pose.position.x = bp.pose[0]
# pch_msg.pose.position.y = bp.pose[1]
# pch_msg.pose.position.z = bp.pose[2]
# pch_msg.pose.orientation.x = q[0]
# pch_msg.pose.orientation.y = q[1]
# pch_msg.pose.orientation.z = q[2]
# pch_msg.pose.orientation.w = q[3]
self.pub.publish(pch_msg)
state_msg = ModelState()
state_msg.model_name = cam_name
# state_msg.pose.position.x = bp.pose[0]
# state_msg.pose.position.y = bp.pose[1]
# state_msg.pose.position.z = bp.pose[2]
# state_msg.pose.orientation.x = q[0]
# state_msg.pose.orientation.y = q[1]
# state_msg.pose.orientation.z = q[2]
# state_msg.pose.orientation.w = q[3]
# TODO: RE-ADD
# rospy.wait_for_service('/gazebo/set_model_state')
print("Sending Gazebo State Message: ")
print(str(state_msg))
# try:
# set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# resp = set_state(state_msg)
# print(str(resp))
# except rospy.ServiceException as e:
# print("Service call failed: %s" % e)
static_transform = geoMsg.TransformStamped()
static_transform.header.stamp = rospy.Time.now()
static_transform.header.frame_id = "world"
static_transform.child_frame_id = frame_name
# static_transform.transform.translation.x = bp.pose[0]
# static_transform.transform.translation.y = bp.pose[1]
# static_transform.transform.translation.z = bp.pose[2]
# static_transform.transform.rotation.x = q[0]
# static_transform.transform.rotation.y = q[1]
# static_transform.transform.rotation.z = q[2]
# static_transform.transform.rotation.w = q[3]
# broadcaster = tf2_ros.StaticTransformBroadcaster()
# broadcaster.sendTransform(static_transform)
count += 1
rospy.loginfo(" CPO Complete")
rospy.loginfo(" Initializing Gazebo Simulation...")
