def _initialise_navigation(self):
initial_x = rospy.get_param('~initial_x', 0.0)
initial_y = rospy.get_param('~initial_y', 0.0)
initial_a = rospy.get_param('~initial_a', 0.0)
pwcs = geometry_msgs.PoseWithCovarianceStamped()
pwcs.pose.pose.position.x = initial_x
pwcs.pose.pose.position.y = initial_y
def _process_localize_goal(self):
goal = self._as_localize.accept_new_goal()
self._distortion = goal.distortion
self.loginfo("Received Localize goal %s" % str(goal))
if self._initialise:
message = 'robot is initialising already. Ignore the command'
self._send_result(False, message)
else:
if self.param['simulation']:
pose_msg = geometry_msgs.PoseWithCovarianceStamped()
pose_msg.header.frame_id = self.param['sim_global_frame']
pose_msg.header.stamp = rospy.Time.now() - rospy.Duration(
0.2) # TODO: get latest common time
pose_msg.pose.pose.position.x = self.param['sim_x']
pose_msg.pose.pose.position.y = self.param['sim_y']
pose_msg.pose.pose.position.z = 0.0
quat = tf.transformations.quaternion_from_euler(
0, 0, self.param['sim_a'])
pose_msg.pose.pose.orientation = geometry_msgs.Quaternion(
*quat)
self._pub_init_pose.publish(pose_msg)
# send success right away
self._send_result(True, 'Initialisation done in simulation.')
elif goal.command == goal.STAND_AND_LOCALIZE:
self._thread = threading.Thread(
target=self._stand_and_localize)
self._thread.start()
elif goal.command == goal.SPIN_AND_LOCALIZE:
self._thread = threading.Thread(target=self._spin_and_localize)
self._thread.start()
else:
message = 'Invalid command %s' % str(goal.command)
self._send_result(False, message)
def __init__(self, odometry_topic='/odom', pose_topic='/pose', duration=5):
super(MoveBase,
self).__init__(action_name="move_base",
action_type=move_base_msgs.MoveBaseAction,
worker=self.worker,
duration=duration)
self.odometry = nav_msgs.Odometry()
self.odometry.pose.pose.position = geometry_msgs.Point(0, 0, 0)
self.pose = geometry_msgs.PoseWithCovarianceStamped()
self.pose.pose.pose.position = geometry_msgs.Point(0, 0, 0)
latched = True
queue_size_five = 1
self.publishers = py_trees_ros.utilities.Publishers([
('pose', pose_topic, geometry_msgs.PoseWithCovarianceStamped,
latched, queue_size_five),
('odometry', odometry_topic, nav_msgs.Odometry, latched,
queue_size_five)
])
self.publishers.pose.publish(self.pose)
self.publishers.odometry.publish(self.odometry)
self.publishing_timer = rospy.Timer(period=rospy.Duration(0.5),
callback=self.publish,
oneshot=False)
self.start()
def initPose():
pub_initialpose = rospy.Publisher('initialpose',
gm.PoseWithCovarianceStamped,
queue_size=100)
rospy.sleep(3.0)
init_pose = gm.PoseWithCovarianceStamped()
init_pose.header.frame_id = 'map'
init_pose.header.stamp = rospy.Time.now()
init_pose.pose.pose.orientation.w = 1
init_pose.pose.covariance = [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0001, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,\
0.0, 0.0, 0.0, 0.0, 0.0, np.deg2rad(1.0)]
pub_initialpose.publish(init_pose)
rospy.sleep(1.0)
def _init_callback(self, msg):
self.loginfo("Initialisation started.")
# enable the pose tracker
if self.param['simulation']:
pose_msg = geometry_msgs.PoseWithCovarianceStamped()
pose_msg.header.frame_id = "ar_global"
pose_msg.header.stamp = rospy.Time.now() - rospy.Duration(0.2) # TODO: get latest common time
pose_msg.pose.pose.position.x = 1.0
pose_msg.pose.pose.position.y = 0.0
pose_msg.pose.pose.position.z = 0.0
quat = tf.transformations.quaternion_from_euler(0, 0, 3.1416)
pose_msg.pose.pose.orientation = geometry_msgs.Quaternion(*quat)
self._pub_init_pose.publish(pose_msg)
# send success right away
empty_msg = std_msgs.Empty()
self._pub_result.publish(empty_msg)
self.loginfo("Initialisation done.")
else:
params = { 'enabled' : 'True' }
config = self._client.update_configuration(params)
self._initialise = True
import geometry_msgs.msg as geo_msg
import rospy
import numpy as np
import tf
import tf2_ros
from scipy.spatial.transform import Rotation
if __name__ == '__main__':
pub = rospy.Publisher('UR_driver_pose', geo_msg.PoseWithCovarianceStamped, queue_size=10)
rospy.init_node('ur)pose_publisher', anonymous=True)
listener = tf.TransformListener()
rate = rospy.Rate(10)
initial_pose = geo_msg.PoseWithCovarianceStamped()
#get transform from world to camera at initialization
while not rospy.is_shutdown():
rate.sleep()
try:
(map_to_pose, map_to_pose_quat) = listener.lookupTransform("/base_link", "/ee_link", rospy.Time(0))
print("found")
initial_pose.header.seq = 1
initial_pose.header.stamp = rospy.get_rostime()
initial_pose.header.frame_id = "base_link"
initial_pose.pose.pose.position.x = map_to_pose[0]
initial_pose.pose.pose.position.y = map_to_pose[1]