def measuredCPCallBack(self, data):
msg = igtlpoint()
msg.name = "Measured"
msg.pointdata = Point(data.pose.position.x * m_to_mm,
data.pose.position.y * m_to_mm,
data.pose.position.z * m_to_mm)
self.igtl_point_pub.publish(msg)
if self.record_started and self.curr_location < self.num_fiducials:
print(self.counter)
# convert pose to array
self.pose[self.curr_location,
self.counter, :, :] = rosPoseToArray(data.pose)
self.counter += 1
# check for length
if self.counter >= self.num_data:
self.counter = 0
self.record_started = False
x_avg = self.pose[self.curr_location, :, 0, 3].mean()
y_avg = self.pose[self.curr_location, :, 1, 3].mean()
z_avg = self.pose[self.curr_location, :, 2, 3].mean()
self.registration_point[self.curr_location, :] = np.array(
[x_avg, y_avg, z_avg])
self.igtl_point_pub.publish(msg)
self.curr_location += 1
printMenu()
def servoCPCallBack(self, data):
msg = igtlpoint()
msg.name = "Servo"
msg.pointdata = Point(data.pose.position.x * m_to_mm,
data.pose.position.y * m_to_mm,
data.pose.position.z * m_to_mm)
self.igtl_point_pub.publish(msg)
def main():
ADC.setup()
cmd_pub = rospy.Publisher('IGTL_POINT_OUT', igtlpoint)
rospy.init_node('cmd', anonymous=True)
rate = rospy.Rate(10) # 10hz
Pointmsg = igtlpoint()
# Pointname = "received"
while(1):
Pointmsg.name="received"
potVal0=ADC.read(analogPin)
potVal=potVal0*1.8
Pointmsg.pointdata.x=float(potVal)
Pointmsg.pointdata.y=float(potVal)
Pointmsg.pointdata.z=float(potVal)
cmd_pub.publish(Pointmsg)
sleep(1)
def __init__(self):
rospy.init_node('dvrk_slicer')
self._mtml = DvrkArm('MTML')
self._mtmr = DvrkArm('MTMR')
self._mtmr_pos_pre = None
self._mtml_pos_pre = None
self._footpedals = DvrkFootPedals()
self._cam_transform = Frame()
self._probe_transform = Frame()
self._igtl_cam_trans = igtltransform()
self._igtl_cam_trans.name = 'CameraTransform'
self._igtl_cam_trans.transform.rotation.w = 1.0
self._igtl_probe_trans = igtltransform()
self._igtl_probe_trans.name = 'ProbeTransform'
self._igtl_probe_trans.transform.rotation.w = 1.0
self._igtl_fiducial_point = igtlpoint()
self._igtl_fiducial_point.name = 'ENTRY'
self._fiducial_offset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self._igtl_text = igtlstring()
self._igtl_cam_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
self._igtl_probe_trans_pub = rospy.Publisher('/IGTL_TRANSFORM_OUT', igtltransform, queue_size=1)
self._igtl_fiducial_pub = rospy.Publisher('/IGTL_POINT_OUT', igtlpoint, queue_size=1)
self._igtl_status_pub = rospy.Publisher('/IGTL_TEXT_OUT', igtlstring, queue_size=1)
self._rate = rospy.Rate(120)
self._pub_msg_pairs = dict()
self._pub_msg_pairs[self._igtl_cam_trans_pub] = self._igtl_cam_trans
self._pub_msg_pairs[self._igtl_probe_trans_pub] = self._igtl_probe_trans
self._pub_msg_pairs[self._igtl_fiducial_pub] = self._igtl_fiducial_point
# self._pub_msg_pairs[self._igtl_status_pub] = self._igtl_text
self._fiducial_placement_modes = ['ENTRY', 'TARGET']
self._fiducial_placement_active_mode = 0
self._cam_init_pose_computed = False
self._r_init_pos = Vector()
self._l_init_pos = Vector()
self._init_vec = Vector()
def igtl_exporter():
rospy.init_node('igtl_exporter', anonymous=True)
pub_igtl_point_out = rospy.Publisher('IGTL_POINT_OUT',
igtlpoint,
queue_size=10)
moveit_commander.roscpp_initialize(sys.argv)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints).
## This interface can be used to plan and execute motions:
group_name = "ur5"
move_group = moveit_commander.MoveGroupCommander(group_name)
listener = tf.TransformListener()
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
wpose = move_group.get_current_pose().pose
end = Point()
end.x = wpose.position.x * 1000
end.y = wpose.position.y * 1000
end.z = wpose.position.z * 1000
end_effector = igtlpoint()
end_effector.name = "Pose"
end_effector.pointdata = end
pub_igtl_point_out.publish(end_effector)
rospy.loginfo(end_effector)
rate.sleep()
def igtl_exporter():
rospy.init_node('igtl_exporter', anonymous=True)
pub_igtl_point_out = rospy.Publisher('IGTL_POINT_OUT',
igtlpoint,
queue_size=10)
#pub_igtl_transform_out = rospy.Publisher('IGTL_TRANSFORM_OUT', igtltransform, queue_size=10)
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = "ur5"
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory,
# queue_size=20)
# Misc variables
#self.box_name = ''
# self.robot = robot
# self.scene = scene
# self.move_group = move_group
# self.display_trajectory_publisher = display_trajectory_publisher
#self.planning_frame = planning_frame
#self.eef_link = eef_link
#self.group_names = group_names
listener = tf.TransformListener()
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
wpose = move_group.get_current_pose().pose
end = Point()
end.x = wpose.position.x * 1000
end.y = wpose.position.y * 1000
end.z = wpose.position.z * 1000
end_effector = igtlpoint()
end_effector.name = "Pose"
end_effector.pointdata = end
pub_igtl_point_out.publish(end_effector)
rospy.loginfo(end_effector)
#log_str = "igtl_exporter %s" % rospy.get_time()
#rospy.loginfo(log_str)
#pub.publish(log_str)
rate.sleep()