class RiCDiffCloseLoop(Device):
def __init__(self, param, output):
Device.__init__(self, param.getCloseDiffName(), output)
self._baseLink = param.getCloseDiffBaseLink()
self._odom = param.getCloseDiffOdom()
self._maxAng = param.getCloseDiffMaxAng()
self._maxLin = param.getCloseDiffMaxLin()
self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
self._broadCase = TransformBroadcaster()
Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
def diffServiceCallback(self, msg):
if msg.angular.z > self._maxAng:
msg.angular.z = self._maxAng
elif msg.angular.z < -self._maxAng:
msg.angular.z = -self._maxAng
if msg.linear.x > self._maxLin:
msg.linear.x = self._maxLin
elif msg.linear.x < -self._maxLin:
msg.linear.x = -self._maxLin
# print msg.angular.z, msg.linear.x
self._output.write(CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend())
def setOdom(self, req):
self._output.write(CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend())
return set_odomResponse(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
self._pub.publish(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._broadCase.sendTransformMessage(traMsg)
class FiducialTFPublisher(object):
def __init__(self, fiducial_topic, fiducial_id, frame_id, invert=False):
self._fiducial_id = fiducial_id
self._frame_id = frame_id
self._invert = invert
self._fid_sub = rospy.Subscriber(fiducial_topic,
FiducialTransformArray,
callback=self._callback,
queue_size=10)
self._tf_brd = TransformBroadcaster()
def _callback(self, fiducial_transforms):
header = fiducial_transforms.header
child_frame_id = self._frame_id
if self._invert:
child_frame_id = header.frame_id
header.frame_id = self._frame_id
obj = filter(lambda a: a.fiducial_id == self._fiducial_id,
fiducial_transforms.transforms)
if len(obj) > 0:
obj = obj[0]
if self._invert:
trans = obj.transform.translation
rot = obj.transform.rotation
trans = [trans.x, trans.y, trans.z]
rot = [rot.x, rot.y, rot.z, rot.w]
m_trans = t.translation_matrix(trans)
m_rot = t.quaternion_matrix(rot)
transform = t.concatenate_matrices(m_trans, m_rot)
inverse_transform = t.inverse_matrix(transform)
trans = t.translation_from_matrix(inverse_transform)
rot = t.quaternion_from_matrix(inverse_transform)
obj.transform.translation = Vector3(x=trans[0],
y=trans[1],
z=trans[2])
obj.transform.rotation = Quaternion(x=rot[0],
y=rot[1],
z=rot[2],
w=rot[3])
self._tf_brd.sendTransformMessage(
TransformStamped(header=header,
child_frame_id=child_frame_id,
transform=obj.transform))
def set_map_pose(pose, parent_frame_id, child_frame_id):
br = TransformBroadcaster()
m = TransformStamped()
m.header.frame_id = parent_frame_id
m.header.stamp = rospy.Time.now()
m.child_frame_id = child_frame_id
m.transform.translation.x = pose.position.x
m.transform.translation.y = pose.position.y
m.transform.translation.z = pose.position.z
m.transform.rotation.x = pose.orientation.x
m.transform.rotation.y = pose.orientation.y
m.transform.rotation.z = pose.orientation.z
m.transform.rotation.w = pose.orientation.w
br.sendTransformMessage(m)
class pathViewer:
def __init__(self, pubName="path"):
self.topicName = pubName
self.interFrameMotions = []
self.br = TransformBroadcaster()
def loadFromFile(self, Dir):
fileNames = os.listdir(Dir)
###add original Transform
origin = TransformStamped()
origin.header.frame_id = "world"
origin.child_frame_id = self.topicName
origin.transform.rotation.w = 1
self.interFrameMotions.append(origin)
count = 0
for f in fileNames:
with open(Dir + "/" + f, "r") as current:
Rtheta, C = pickle.load(current)
q = quaternion_from_euler(radians(Rtheta[0]),
radians(Rtheta[1]),
radians(Rtheta[2]), 'szxy')
count += 1
print(f)
latestPose = TransformStamped()
latestPose.header.frame_id = self.interFrameMotions[
-1].child_frame_id
latestPose.child_frame_id = self.topicName + "/" + f[:f.rfind(
".")]
latestPose.transform.translation.x = C[0, 0]
latestPose.transform.translation.y = C[1, 0]
latestPose.transform.translation.z = C[2, 0]
latestPose.transform.rotation.x = q[0]
latestPose.transform.rotation.y = q[1]
latestPose.transform.rotation.z = q[2]
latestPose.transform.rotation.w = q[3]
self.interFrameMotions.append(latestPose)
def publish(self):
for i in self.interFrameMotions:
i.header.stamp = rospy.Time.now()
self.br.sendTransformMessage(i)
class TagTransformer(object):
"""
A TagTransformer is responsible for transforming detections by the apriltags_ros library
into RoombaList messages for processing by the rest of the stack.
"""
def __init__(self,
linear_covariance=0.1,
angular_covariance=0.5,
tf_frame='odom',
camera_fov=1.0):
"""
:param linear_covariance: (meters) the linear covariance associated with a tag detection
:param angular_covariance: (radians) the angular covariance of a tag
:param tf_frame (str): The TF frame of the map / world.
:param camera_fov (float): The FOV radius of the camera, in meters.
"""
self.tf = TransformListener()
self.pub = rospy.Publisher('visible_roombas', RoombaList, queue_size=0)
self.tf_pub = TransformBroadcaster()
cov = [0] * 36
cov[0] = cov[7] = cov[14] = linear_covariance
cov[21] = cov[28] = cov[35] = angular_covariance
self.covariance = cov
self.fov = camera_fov
self.map_frame = tf_frame
# Negates x and y coordinates of tag detections
self.apply_apriltag_fix = True
rospy.Subscriber('tag_detections', AprilTagDetectionArray,
self.on_tags)
def on_tags(self, msg):
"""
Handler for AprilTag detection.
Converts AprilTagDetectionArray to a RoombaList object, then publishes it.
:type msg: AprilTagDetectionArray
"""
sighting = RoombaList()
for tag in msg.detections: # type: AprilTagDetection
if self.apply_apriltag_fix:
tag.pose.pose.position.x *= -1
tag.pose.pose.position.y *= -1
roomba_frame_id = 'roombas/{}'.format(tag.id)
# Transform the detection into map frame, and constrain it to be flat on the ground
pose = self.tf.transformPose(self.map_frame, tag.pose)
pose.pose.position.z = 0
_, _, z_angle = euler_from_quaternion(
[getattr(pose.pose.orientation, s) for s in 'xyzw'])
pose.pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, z_angle))
roomba = Roomba()
roomba.visible_location = PoseWithCovarianceStamped(
header=pose.header,
pose=PoseWithCovariance(pose=pose.pose,
covariance=self.covariance))
# Because April tags don't have color information, assign even tags to be GREEN and odd tags to be RED
roomba.type = [Roomba.GREEN, Roomba.RED][tag.id % 2]
roomba.last_seen = tag.pose.header.stamp
roomba.frame_id = roomba_frame_id
roomba.magical_id = tag.id + 100
sighting.data.append(roomba)
# self.tf_pub.sendTransform(roomba.visible_location.pose.pose.position,
# roomba.visible_location.pose.pose.orientation,
# roomba.last_seen, roomba_frame_id, self.tf_frame)
self.tf_pub.sendTransformMessage(
TransformStamped(
header=roomba.visible_location.header,
child_frame_id=roomba.frame_id,
transform=Transform(
translation=roomba.visible_location.pose.pose.position,
rotation=roomba.visible_location.pose.pose.orientation)
))
self.pub.publish(sighting)
class RiCDiffCloseLoop(Device):
def __init__(self, param, output):
Device.__init__(self, param.getCloseDiffName(), output)
self._baseLink = param.getCloseDiffBaseLink()
self._odom = param.getCloseDiffOdom()
self._maxAng = param.getCloseDiffMaxAng()
self._maxLin = param.getCloseDiffMaxLin()
self._pub = Publisher("%s/odometry" % self._name,
Odometry,
queue_size=param.getCloseDiffPubHz())
self._broadCase = TransformBroadcaster()
Subscriber('%s/command' % self._name,
Twist,
self.diffServiceCallback,
queue_size=1)
Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
self._haveRightToPublish = False
self._prevOdom = None
self._firstTimePub = True
def getType(self):
return DiffDriverCL
def diffServiceCallback(self, msg):
Thread(target=self.sendMsg, args=(msg, )).start()
def sendMsg(self, msg):
if msg.angular.z > self._maxAng:
msg.angular.z = self._maxAng
elif msg.angular.z < -self._maxAng:
msg.angular.z = -self._maxAng
if msg.linear.x > self._maxLin:
msg.linear.x = self._maxLin
elif msg.linear.x < -self._maxLin:
msg.linear.x = -self._maxLin
# print msg.angular.z, msg.linear.x
self._output.write(
CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend())
def setOdom(self, req):
self._output.write(
CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend())
return set_odomResponse(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 checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*',
rostopic.get_info_text(
self._pub.name)).group(0).split(': ')[1]
subTfCheck = re.search(
'Subscribers:.*',
rostopic.get_info_text('/tf')).group(0).split(': ')[1]
if not self._haveRightToPublish and (subCheck == ''
or subTfCheck == ''):
self._output.write(
PublishRequest(DiffDriverCL, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and (subCheck == 'None'
and subTfCheck == 'None'):
self._output.write(
PublishRequest(DiffDriverCL, 0, False).dataTosend())
self._haveRightToPublish = False
self._firstTimePub = True
except:
pass
class OdomNode:
def __init__(self):
self.const = 0.05235987755
self.left_value = None
self.rigth_value = None
self.x_pos = 0.0
self.y_pos = 0.0
self.l_vel = 0.0
self.a_vel = 0.0
self.heading = 0
self.left_sub = rospy.Subscriber("/left_encoder_value",
Int64,
self.callback_left_encoder,
buff_size=30)
self.right_sub = rospy.Subscriber("/right_encoder_value",
Int64,
self.callback_right_encoder,
buff_size=30)
self.odom_publisher = rospy.Publisher("odom", Odometry, queue_size=50)
self.last_time = rospy.Time.now()
self.odom_broadcaster = TransformBroadcaster()
def callback_left_encoder(self, msg):
self.left_value = msg.data
pass
def callback_right_encoder(self, msg):
self.right_value = msg.data
self.publish_odometry()
pass
def publish_odometry(self):
current_time = rospy.Time.now()
omega_l = (self.left_value * self.const)
omega_r = (self.right_value * self.const)
self.l_vel = ((omega_l + omega_r) / 2.0) * 0.062
self.a_vel = -(omega_l - omega_r) * (0.062 / 0.45)
vel_dt = (current_time - self.last_time).to_sec()
delta_heading = self.a_vel * vel_dt
delta_x = (self.l_vel * np.cos(self.heading)) * vel_dt
delta_y = (self.l_vel * np.sin(self.heading)) * vel_dt
#print(self.x_pos, self.y_pos, self.heading, self.l_vel, self.a_vel, self.left_value, self.right_value, vel_dt)
self.x_pos += delta_x
self.y_pos += delta_y
self.heading += delta_heading # TF2 heading to quaternion
quat = quaternion_from_euler(0, 0, self.heading)
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 = self.x_pos
odom_trans.transform.translation.y = self.y_pos
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation.x = quat[0]
odom_trans.transform.rotation.y = quat[1]
odom_trans.transform.rotation.z = quat[2]
odom_trans.transform.rotation.w = quat[3]
#self.odom_broadcaster.sendTransform(odom_trans)
self.odom_broadcaster.sendTransformMessage(odom_trans)
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = self.x_pos
odom.pose.pose.position.y = self.y_pos
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation.x = quat[0]
odom.pose.pose.orientation.y = quat[1]
odom.pose.pose.orientation.z = quat[2]
odom.pose.pose.orientation.w = quat[3]
odom.pose.covariance[0] = 0.0001
odom.pose.covariance[7] = 0.0001
odom.pose.covariance[35] = 0.0001
odom.twist.twist.linear.x = self.l_vel
odom.twist.twist.linear.y = 0.0
odom.twist.twist.linear.z = 0.0
odom.twist.twist.angular.x = 0.0
odom.twist.twist.angular.y = 0.0
odom.twist.twist.angular.z = self.a_vel
odom.twist.covariance[0] = 0.0001
odom.twist.covariance[7] = 0.0001
odom.twist.covariance[35] = 0.0001
self.odom_publisher.publish(odom)
self.last_time = current_time
class RiCDiffCloseLoop(Device):
def __init__(self, param, output):
Device.__init__(self, param.getCloseDiffName(), output)
self._baseLink = param.getCloseDiffBaseLink()
self._odom = param.getCloseDiffOdom()
self._maxAng = param.getCloseDiffMaxAng()
self._maxLin = param.getCloseDiffMaxLin()
self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
self._broadCase = TransformBroadcaster()
Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
self._haveRightToPublish = False
self._prevOdom = None
self._firstTimePub = True
def getType(self):
return DiffDriverCL
def diffServiceCallback(self, msg):
Thread(target=self.sendMsg, args=(msg,)).start()
def sendMsg(self, msg):
if msg.angular.z > self._maxAng:
msg.angular.z = self._maxAng
elif msg.angular.z < -self._maxAng:
msg.angular.z = -self._maxAng
if msg.linear.x > self._maxLin:
msg.linear.x = self._maxLin
elif msg.linear.x < -self._maxLin:
msg.linear.x = -self._maxLin
# print msg.angular.z, msg.linear.x
self._output.write(CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend())
def setOdom(self, req):
self._output.write(CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend())
return set_odomResponse(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 checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
subTfCheck = re.search('Subscribers:.*', rostopic.get_info_text('/tf')).group(0).split(': ')[1]
if not self._haveRightToPublish and (subCheck == '' or subTfCheck == ''):
self._output.write(PublishRequest(DiffDriverCL, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and (subCheck == 'None' and subTfCheck == 'None'):
self._output.write(PublishRequest(DiffDriverCL, 0, False).dataTosend())
self._haveRightToPublish = False
self._firstTimePub = True
except:
pass
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("/postImage",Image,queue_size=50)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback)
self.h = Header()
self.tb = TransformBroadcaster()
self.img_tf=TransformStamped()
def callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows,cols,channels) = cv_image.shape
cv2.circle(cv_image, (320,240), 10, (0,255,255),-1)
(corners, ids, rejected) = cv2.aruco.detectMarkers(cv_image,arucoDict, parameters=arucoParams)
frame_markers = cv2.aruco.drawDetectedMarkers(cv_image.copy(), corners, ids)
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners, marker_size, mtx, dist)
# print("trans",tvec)
# print("rot",rvec)
# cv2.aruco.drawAxis(cv_image,mtx,dist,rvec,tvec,0.1)
cv2.aruco.drawAxis(frame_markers,mtx,dist,rvec,tvec,0.05)
cv2.putText(frame_markers,np.array_str(tvec),(0,50),cv2.FONT_HERSHEY_SIMPLEX,0.7,(0,0,255),2)
# cv2.imshow("Image window", frame_markers)
# cv2.imshow("Image window", cv_image)
# cv2.waitKey(3)
self.tf_pub(tvec,rvec)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame_markers, "bgr8"))
except CvBridgeError as e:
print(e)
def tf_pub(self,tvec,rvec):
# q=tf.quaternion_from_euler(0,-pi/2,-pi/2,axes="rxyz")
# q=tf.quaternion_from_euler(pi/2,0,-pi/2,axes="sxyz")
q=tf.quaternion_from_euler(rvec[0][0][0],rvec[0][0][1],rvec[0][0][2],axes="sxyz")
print(rvec)
self.img_tf.transform.translation.y=-tvec[0][0][0];
self.img_tf.transform.translation.z=-tvec[0][0][1];
self.img_tf.transform.translation.x=tvec[0][0][2];
# self.img_tf.transform.rotation.w=1;
self.img_tf.transform.rotation.x=q[0];self.img_tf.transform.rotation.y=q[1];
self.img_tf.transform.rotation.z=q[2];self.img_tf.transform.rotation.w=q[3];
self.h.frame_id="cam_link"
self.img_tf.header=self.h
self.img_tf.child_frame_id="img_tf"
self.h.stamp = rospy.Time.now()
self.tb.sendTransformMessage(self.img_tf)
class moveit_interface:
def __init__(self, config):
rospy.loginfo("Start to intialize moveit_control_interface")
# Config
self.cfg = config.cfg
# TF
self.tf = TransformListener()
self.br = TransformBroadcaster()
rospy.Subscriber(self.cfg['topicMoveGroupResult'],
MoveGroupActionResult, self.cb_move_group_result)
rospy.Subscriber(self.cfg['topicTrajectoryExecutionResult'],
ExecuteTrajectoryActionResult,
self.cb_trajectory_execution_result)
# Wait for Moveit action server
moveit_interface_found = False
while not moveit_interface_found:
try:
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander(
self.cfg['move_group_name'])
moveit_interface_found = True
except Exception as e:
rospy.logerr(e.message)
moveit_interface_found = False
rospy.logerr("Retrying to connect to MoveIt action server ...")
rospy.signal_shutdown('No MoveIt interface found')
return
# Create an inverse mapping of the joint names
self.active_joints = self.group.get_active_joints()
rospy.loginfo("============ Active joints: %d" %
len(self.active_joints))
self.active_joints_id = {}
i = 0
for joint_name in self.active_joints:
self.active_joints_id[i] = joint_name
i += 1
rospy.loginfo(self.active_joints_id)
# Joint/Goal tolerances for motion planning:
rospy.loginfo(
"============ Goal tolerances: joint, position, orientation")
self.group.set_goal_position_tolerance(
self.cfg['goal_position_tolerance'])
self.group.set_goal_joint_tolerance(self.cfg['goal_joint_tolerance'])
self.group.set_goal_orientation_tolerance(
self.cfg['goal_orientation_tolerance'])
rospy.loginfo("Joint-, position-, orientation tolerance: ")
rospy.loginfo(
self.group.get_goal_tolerance()
) # Return a tuple of goal tolerances: joint, position and orientation.
rospy.loginfo("============ Reference frame for poses of end-effector")
rospy.loginfo(self.group.get_pose_reference_frame())
## Publish a goal to TF
def _publish_tf(self, poseStamped, name="moveit_target"):
transform = TransformStamped()
transform.header = poseStamped.header
transform.transform.translation = poseStamped.pose.position
transform.transform.rotation = poseStamped.pose.orientation
transform.child_frame_id = name
self.br.sendTransformMessage(transform)
## Init the MoveIt planning scene
# Important: the moveit_commander needs some time to come up!
def init_planning_scene(self, remove_objects=False, addGround=False):
rospy.loginfo("Initializing the planning scene")
rospy.sleep(2.0)
if remove_objects:
rospy.loginfo("Removing all objects from the planning scene")
self.scene.remove_world_object()
if addGround:
if not self.add_ground("ground", 0.0):
rospy.logerr("Ground was not added to the scene")
def _add_object_decorator(func):
def wrapper(*args, **kwargs):
self = args[0]
rospy.loginfo("Adding object '" + args[1] + "' to the scene.")
func(*args, **kwargs)
res = self.wait_for_object(args[1])
rospy.loginfo("Known objects in the scene: " +
str(self.scene.get_known_object_names()))
return res
return wrapper
## Adds a horizontal ground plane to the planning scene
# @param name Name of the ground plane
# @param z z-Position of the ground plane
@_add_object_decorator
def add_ground(self, name, z=0.0, frame=""):
p = geometry_msgs.msg.PoseStamped()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = z
p.pose.orientation.w = 1.0
if frame is not "":
p.header.frame_id = frame
else:
p.header.frame_id = self.robot.get_planning_frame()
self.scene.add_plane(name, p)
## Add a box to the planning scene
# @param name Name of the Object
# @param size Size of the object given as vector list (dx,dy,dz)
# @param position Position of the object given as vector list (x,y,z)
# @param orientation Orientation of the object given as quaternion list (x,y,z,w)
# @param frame Attach to the given frame
@_add_object_decorator
def add_box(self, name, size, position, orientation, frame=""):
p = geometry_msgs.msg.PoseStamped()
p.pose.position = geometry_msgs.msg.Point(position[0], position[1],
position[2])
p.pose.orientation = geometry_msgs.msg.Quaternion(
orientation[0], orientation[1], orientation[2], orientation[3])
if frame is not "":
p.header.frame_id = frame
else:
p.header.frame_id = self.robot.get_planning_frame()
self.scene.add_box(name, p, size)
## Wait for timeout s if an object appears in the planning scene
# @param object_name Nome of the object to check/wait for
# @param timeout Wait for timeout s
def wait_for_object(self, object_name, timeout=3.0):
start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
# Test if we are in the expected state
if object_name in self.scene.get_known_object_names():
return True
# Sleep so that we give other threads time on the processor
rospy.loginfo("Waiting for scene to be updated")
rospy.sleep(0.5)
seconds = rospy.get_time()
rospy.logerr("Object '" + object_name + "' was not found in the scene")
return False
## Execute the plan passed as parameter or, if the passed plan is None, execute the most recently planned trajectory.
def execute(self, plan=None, wait=True):
if plan is None:
self.group.set_start_state_to_current_state()
plan = self.group.plan()
if len(plan.joint_trajectory.points) is 0:
rospy.logerr(
"Empty trajectory, nothing to execute! Probably IK did not find a solution."
)
return
self.group.execute(plan, wait)
#self.group.go(wait=True)
## Change the current pose in the direction of the passed axis by the passed value.
# This is a helper function for the move_x/y/z and rotate_y/y/z methods.
# @param axis The axis to change (integer from 0 to 5 corresponding to the translation/rotation axis)
# @param value The value to be added to the current pose value.
def _shift_pose_target(self, axis, value, wait=True):
try:
self.group.shift_pose_target(axis, value)
self.execute(wait=wait)
except Exception as e:
rospy.logerr(e.message)
return
## Move to a new 6D endeffector pose (geometry_msgs/Pose)
def move_pose(self, pose, wait=True):
rospy.loginfo("Received pose command")
try:
self.group.set_pose_target(pose)
self._publish_tf(pose)
self.execute(wait=wait)
except Exception as e:
rospy.logerr(e.message)
return
## Move to a new 3D endeffector position (geometry_msgs/Point)
def move_position(self, position, wait=True):
try:
rospy.loginfo("Received position command")
self.group.set_position_target(
[position.x, position.y, position.z])
self.execute(wait=wait)
except Exception as e:
rospy.logerr(e.message)
return
## Move to a named (and pre-defined) fixed pose
def move_fixed_pose(self, fixed_pose_name, wait=True):
rospy.loginfo("Received fixed pose command: %s" % fixed_pose_name)
try:
self.group.set_named_target(fixed_pose_name)
self.execute(wait=wait)
except Exception as e:
rospy.logerr(e.message)
return
## Move to a given joint state
def move_joint_state(self, joint_state, wait=True):
rospy.loginfo("Received joint state command")
desired_joint_pos = None
if len(joint_state.name) is not 0 and len(joint_state.name) is len(
joint_state.position):
desired_joint_pos = dict(
zip(joint_state.name, joint_state.position))
elif len(joint_state.name) is 0 and len(joint_state.position) is len(
self.active_joints):
desired_joint_pos = joint_state.position
else:
rospy.logerr("Invalid joint state message received! %s" %
joint_state)
if desired_joint_pos is not None:
try:
self.group.set_joint_value_target(desired_joint_pos)
self.execute(wait=wait)
except Exception as e:
rospy.logerr(e.message)
return
## Move the endeffector relative in the x-direction
def move_x(self, move_by_value, wait=True):
rospy.loginfo("Received move x command: %f" % move_by_value)
self._shift_pose_target(0, move_by_value, wait)
return
## Move the endeffector relative in the x-direction in a straight line
def move_x_straigt(self, move_by_value, wait=True):
rospy.loginfo("Received move x straight command: %f" % move_by_value)
pose = self.group.get_current_pose().pose
pose.position.x += move_by_value
self.move_cartesian_path_to_pose(pose)
return
## Move the endeffector relative in the y-direction
def move_y(self, move_by_value, wait=True):
rospy.loginfo("Received move y command: %f" % move_by_value)
self._shift_pose_target(1, move_by_value, wait)
return
## Move the endeffector relative in the y-direction in a straight line
def move_y_straigt(self, move_by_value, wait=True):
rospy.loginfo("Received move y straight command: %f" % move_by_value)
pose = self.group.get_current_pose().pose
pose.position.y += move_by_value
self.move_cartesian_path_to_pose(pose)
return
## Move the endeffector relative in the z-direction
def move_z(self, move_by_value, wait=True):
rospy.loginfo("Received move z command: %f" % move_by_value)
self._shift_pose_target(2, move_by_value, wait)
return
## Move the endeffector relative in the z-direction in a straight line
def move_z_straigt(self, move_by_value, wait=True):
rospy.loginfo("Received move z straight command: %f" % move_by_value)
pose = self.group.get_current_pose().pose
pose.position.z += move_by_value
self.move_cartesian_path_to_pose(pose)
return
## Relative rotation of the endeffector around the x-axis
def rotate_x(self, rotate_by_value, wait=True):
rospy.loginfo("Received rotate x command: %f" % rotate_by_value)
self._shift_pose_target(3, rotate_by_value, wait)
return
## Relative rotation of the endeffector around the y-axis
def rotate_y(self, rotate_by_value, wait=True):
rospy.loginfo("Received rotate y command: %f" % rotate_by_value)
self._shift_pose_target(4, rotate_by_value, wait)
return
## Relative rotation of the endeffector around the z-axis
def rotate_z(self, rotate_by_value, wait=True):
rospy.loginfo("Received rotate z command: %f" % rotate_by_value)
self._shift_pose_target(5, rotate_by_value, wait)
return
## Move the endeffector to a named TF pose
def move_to_tf_pose(self, tf_frame_name, wait=True):
rospy.loginfo("Received move to TF pose command: %s" %
tf_frame_name.data)
pose_reference_frame = self.group.get_pose_reference_frame()
try:
t = self.tf.getLatestCommonTime(pose_reference_frame,
tf_frame_name.data)
pos, rot = self.tf.lookupTransform(pose_reference_frame,
tf_frame_name.data, t)
self.group.set_pose_target(pos + rot)
self.execute(wait=wait)
except Exception as e:
rospy.logerr(e.message)
return
## Move the endeffector to the position of a named TF pose
def move_to_tf_position(self, tf_frame_name, wait=True):
rospy.loginfo("Received move to TF position command: %s" %
tf_frame_name.data)
pose_reference_frame = self.group.get_pose_reference_frame()
try:
t = self.tf.getLatestCommonTime(pose_reference_frame,
tf_frame_name.data)
pos, rot = self.tf.lookupTransform(pose_reference_frame,
tf_frame_name.data, t)
self.group.set_position_target(pos)
self.execute(wait=wait)
except Exception as e:
rospy.logerr(e.message)
return
## Move the endeffector on a cartesian path (straight line) to the goal pose
def move_cartesian_path_to_pose(self, pose, wait=True):
try:
rospy.loginfo("Received move pose with cartesian path")
waypoints = []
current_pose = self.group.get_current_pose()
waypoints.append(copy.deepcopy(current_pose.pose))
waypoints.append(copy.deepcopy(pose))
# waypoints to follow, eef_step, jump_threshold
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, 0.01, 0.0)
self.execute(plan, wait)
except Exception as e:
rospy.logerr(e.message)
return
## Get the current pose of the robot
def get_current_pose(self):
try:
return self.group.get_current_pose().pose
except Exception as e:
rospy.logerr(e.message)
return None
## Callback function for move group planning action result feedback
def cb_move_group_result(self, result):
rospy.loginfo("Move group: %s (code %d)" %
(result.status.text, result.status.status))
## Callback function for trajectory execution action result feedback
def cb_trajectory_execution_result(self, result):
rospy.loginfo("Trajectory execution: %s (code %d)" %
(result.status.text, result.status.status))
