def read_topic(self, my_topic, device):
device = device.lower()
print 'my_topic = ' + my_topic.__str__()
time_diff = rospy.Duration(1)
time_previous_message = None
# FIXME: The current timing code will probably have an increasing delay as messages are played.
# We should fix this by basing the sleep duration on the difference between
# the current wall clock and the desired time of the message
print( '\nmsg count = ' + self.bag.get_message_count().__str__())
bag_messages = self.bag.read_messages( topics=[my_topic])
for topic, msg, time_current_message in bag_messages:
if device == "hardware":
if self.read_count == 0:
self.set_state(topic, 'DVRK_POSITION_GOAL_JOINT')
elif self.read_count == 3:
self.set_state(topic, 'DVRK_POSITION_JOINT')
self.read_count = self.read_count + 1
# print(self.read_count)
# the timestamp of each recorded message is saved as time_current_message
if time_previous_message != None:
time_diff = time_current_message-time_previous_message
temp_t = time.time()
sleep(time_diff)
# s = raw_input('\nPress any key to continue...(except q)')
# if s == 'q':
# return
print('time.time says {}\n'.format(time.time() - temp_t))
print( 'time_diff = {} '.format(time_diff.to_sec() ) )
# print('time_previous_message = {}, rostime = {}'.format(time_previous_message, rospy.rostime.get_rostime()))
time_previous_message= time_current_message
if 'PSM1' in topic or 'PSM2' in topic:
msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
elif 'ECM' in topic:
msg.name = ['outer_yaw', 'outer_pitch', 'insertion', 'outer_roll']
# publish
if self.read_count < 3 and device == "hardware":
print('HOMING..\n')
for arm_name in ARM_NAMES:
if arm_name in topic:
print(arm_name + topic)
rospy.Publisher('/dvrk/{}/set_position_goal_joint'.format(arm_name), JointState, queue_size=10).publish(msg)
sleep(2)
else:
if device == "simulation":
self.pub_sim[topic].publish(msg)
if device == "hardware":
msg.position = [ round(i,4) for i in msg.position]
self.pub_hw[topic].publish(msg)
def __init__(self, arm_names):
if type(arm_names) == type(""):
arm_names = [arm_names]
# initialize a bag
self.bag = rosbag.Bag('test.bag', 'w')
self.arm_names = arm_names
# The topics we want to record
# self.topics = '/dvrk/{}/position_joint_current'.format(ARM_NAME)
self.topics = {
arm_name: '/dvrk/{}/position_joint_current'.format(arm_name)
for arm_name in self.arm_names
}
self.out_topics_hw = {
arm_name: '/dvrk/{}/set_position_joint'.format(arm_name)
for arm_name in self.arm_names
}
self.out_topics_sim = {
arm_name: '/dvrk_{}/joint_states_robot'.format(arm_name.lower())
for arm_name in self.arm_names
}
# self.out_topics = self.out_topics_sim
self.out_topics = self.topics
print self.topics
# We have to initialize a ros node if we want to subsribe or publish messages
rospy.init_node('rosbag_test_node')
for arm_name in self.arm_names:
eval("rospy.Subscriber('{}', JointState, self.cb_{})".format(
self.topics[arm_name], arm_name))
sleep(.1)
rospy.spin()
def writeTodbm(self):
self.db = dbm.open(self.db_file, 'w')
self.timer = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d %H:%M:%S')
self.db['timer'] = self.timer
#写能量数据
self.db['eg_total_sum'] = str(self.eg_total_sum)
self.db['eg_min_sum'] = str(self.eg_min_sum)
self.db['eg_max_sum'] = str(self.eg_max_sum)
#写过零数据
self.db['zc_total_sum'] = str(self.zc_total_sum)
self.db['zc_min_sum'] = str(self.zc_min_sum)
self.db['zc_max_sum'] = str(self.zc_max_sum)
#如果误差范围很小，则可以休眠一段时间，并将消息写入json文件
eg_err = float(
self.db['eg_min_avg']) - (self.eg_min_sum / self.eg_numbers * 10)
zc_err = float(
self.db['zc_min_avg']) - (self.zc_min_sum / self.zc_numbers * 10)
self.db['eg_total_avg'] = str(self.eg_total_sum / self.eg_numbers)
self.db['eg_min_avg'] = str(self.eg_min_sum / self.eg_numbers * 10)
self.db['eg_max_avg'] = str(self.eg_max_sum / self.eg_numbers * 10)
self.db['eg_numbers'] = str(self.eg_numbers)
self.db['zc_total_avg'] = str(self.zc_total_sum / self.zc_numbers)
self.db['zc_min_avg'] = str(self.zc_min_sum / self.zc_numbers * 10)
self.db['zc_max_avg'] = str(self.zc_max_sum / self.zc_numbers * 10)
self.db['zc_numbers'] = str(self.zc_numbers)
#若同时收敛，则写如json 文件并且发布消息
if ((-0.01 < eg_err and eg_err < 0.01)
and (-1 < zc_err and zc_err < 1)):
with open(os.path.join(self.wkdir, "eg_zc_average.json"),
'w') as fd:
self.ezdir["eg_total_avg"] = float(self.db['eg_total_avg'])
self.ezdir['eg_min_avg'] = float(self.db['eg_min_avg'])
self.ezdir['eg_max_avg'] = float(self.db['eg_max_avg'])
self.ezdir["zc_total_avg"] = float(self.db['zc_total_avg'])
self.ezdir['zc_min_avg'] = float(self.db['zc_min_avg'])
self.ezdir['zc_max_avg'] = float(self.db['zc_max_avg'])
fd.write(json.dumps(self.ezdir))
fd.close()
self.state = True
if self.state:
self.pub.publish("update")
self.state = False
sleep(1)
self.db.close()
def LaunchNode(packageName, launchFile, nodesList, timeOut=10):
print "launching: " + launchFile
my_env = os.environ.copy()
my_env["TURTLEBOT_3D_SENSOR"] = "commonsense"
my_env["TURTLEBOT_STACKS"] = "hexagons"
proc = subprocess.Popen(['roslaunch', packageName, launchFile], env=my_env)
allNodes = 0
while allNodes != len(nodesList) and timeOut > 0:
allNodes = 0
sleep(1)
for node in nodesList:
if rosnode.rosnode_ping(node, 1, False) == True:
allNodes = allNodes + 1
else:
break
timeOut = timeOut - 1
print "Process pid: " + str(proc.pid)
return proc.pid
def __init__(self):
# initialize a bag
self.bag = rosbag.Bag('test.bag', 'w')
# The topics we want to record
# self.topics = '/dvrk/{}/position_joint_current'.format(ARM_NAME)
self.topics = { arm_name:'/dvrk/{}/position_joint_current'.format(arm_name) for arm_name in ARM_NAMES}
self.out_topics = {arm_name : '/dvrk/{}/set_position_joint'.format(arm_name) for arm_name in ARM_NAMES}
self.out_topics = self.topics
print self.topics
# We have to initialize a ros node if we want to subsribe or publish messages
rospy.init_node('rosbag_test_node')
for arm_name in ARM_NAMES:
eval("rospy.Subscriber('{}', JointState, self.cb_{})".format(self.topics[arm_name], arm_name))
sleep(.1)
rospy.spin()
def main():
a = rospy.init_node('set_base_frames')
global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot, mtmr_robot, mtmr_kin
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)
ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
if mtmr_robot is None:
mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
mtmr_base = KDLKinematics(mtmr_robot, mtmr_robot.links[0].name, mtmr_robot.links[1].name)
# pdb.set_trace()
psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, latch=True, queue_size=1)
psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, latch=True, queue_size=1)
mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, latch=True, queue_size=1)
ecm_tip = ecm_kin.forward([1,1,1,1]) # ECM Tool Tip
ecm_tip = np.linalg.inv(ecm_tip)
psm1_base_frame = psm1_kin.forward([])
psm1_message = pose_converter.PoseConv.to_pose_msg(psm1_base_frame)
psm2_base_frame = psm2_kin.forward([])
psm2_message = pose_converter.PoseConv.to_pose_msg(psm2_base_frame)
psm1_pub.publish(psm1_message)
psm2_pub.publish(psm2_message)
# mtmr_pub.publish(message)
print (psm1_message)
print (psm2_message)
sleep (1)
def __init__(self, arm_names):
if type(arm_names) == type(""):
arm_names = [arm_names]
# initialize a bag
self.bag = rosbag.Bag('test.bag', 'w')
self.arm_names = arm_names
# The topics we want to record
# self.topics = '/dvrk/{}/position_joint_current'.format(ARM_NAME)
self.topics = { arm_name:'/dvrk/{}/position_joint_current'.format(arm_name) for arm_name in self.arm_names}
self.out_topics_hw = {arm_name : '/dvrk/{}/set_position_joint'.format(arm_name) for arm_name in self.arm_names}
self.out_topics_sim = {arm_name : '/dvrk_{}/joint_states_robot'.format(arm_name.lower()) for arm_name in self.arm_names}
# self.out_topics = self.out_topics_sim
self.out_topics = self.topics
print self.topics
# We have to initialize a ros node if we want to subsribe or publish messages
rospy.init_node('rosbag_test_node')
for arm_name in self.arm_names:
eval("rospy.Subscriber('{}', JointState, self.cb_{})".format(self.topics[arm_name], arm_name))
sleep(.1)
rospy.spin()
def move(timer_event):
"""
handles the movement
"""
global current_state
global is_currently_moving
if rospy.get_param("KEY"):
# wait if there is a keyboard command
sleep(2)
if is_currently_moving and (not movement_is_ok()):
return
if current_state == FWD:
move_robot(DISTANCE_BEFORE_TURN)
current_state = TRN
else:
# calculate a random angle to turn
angle = random.randrange(-MAX_TURNING_ANGLE, MAX_TURNING_ANGLE)
clockwise = angle < 0
turn_robot(angle,clockwise)
current_state = FWD
def main():
rospy.init_node('set_base_frames')
sleep (1)
global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)
ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
# pdb.set_trace()
mtml_pub = rospy.Publisher('/dvrk/MTML/set_base_frame', Pose, queue_size=1)
mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, queue_size=1)
ecm_pub = rospy.Publisher('/dvrk/ECM/set_base_frame', Pose, queue_size=1)
psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, queue_size=1)
psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, queue_size=1)
mtmr_psm1 = rospy.Publisher('/dvrk/MTMR_PSM1/set_registration_rotation', Quaternion, queue_size=1)
mtml_psm2 = rospy.Publisher('/dvrk/MTML_PSM2/set_registration_rotation', Quaternion, queue_size=1)
p1_base = psm1_kin.forward([]) # PSM1 Base Frame
p2_base = psm2_kin.forward([]) # PSM2 Base Frame
e_base = ecm_base.forward([]) # ECM Base Frame
e = ecm_kin.forward([0,0,0,0]) # ECM Tool Tip
camera_view_transform = pose_converter.PoseConv.to_homo_mat( [(0.0,0.0,0.0), (1.57079632679, 0.0, 0.0)])
r = lambda axis, rad: rotate(axis, rad)
mtmr_m = e;# mtmr_m = mtmr_m**-1
mtml_m = e
print 'qmat'
qmsg = Quaternion()
temp = quat_from_homomat(p1_base)
print quat_from_homomat(p1_base)
message = pose_from_homomat(p1_base);
psm1_pub.publish(message)
while not rospy.is_shutdown():
#print p1_base
#print message
psm1_pub.publish(message)
print ('sure\n')
# psm2_pub.publish(pose_from_homomat(p2_base))
# psm1_pub.publish(pose_from_homomat(e_base))
# mtml_pub.publish( pose_from_homomat(mtml_m))
# mtmr_pub.publish( pose_from_homomat(mtmr_m))
print ('\n\Hello: nmtml rotation: \n')
print( mtml_m.__repr__( ))
print(pose_from_homomat(mtml_m))
print ('\n\nmtmr rotation: \n')
print( mtmr_m.__repr__())
def run_as_process(aut: AutomatonBase, i_queue: Queue, o_queue: Queue,
stop_ev: Event, **kwargs: Any) -> None:
""" NOTE: Must be executed in the main thread of a process """
start_time = float("NaN")
try:
rospy.init_node(repr(aut), anonymous=True, disable_signals=False)
rospy.logdebug("%s: waiting for the first clock message" % repr(aut))
rospy.wait_for_message("/clock", Clock) # Wait for first update of clock
# Initialize Publishers and Subscribers
# FIXME TIOA should not depend on specific implementations
from .agent import Agent
if isinstance(aut, Agent):
aut.motion.register_ros_pub_sub()
pose_topic_name = "/vrpn_client_node/%s/pose" % str(aut.uid)
def update_pose(data: PoseStamped):
aut.motion.position = data.pose.position
aut.motion.orientation = data.pose.orientation
# NOTE This creates a thread in this process
rospy.Subscriber(pose_topic_name, PoseStamped, update_pose, queue_size=10)
rospy.logdebug("%s: waiting for the first position message" % repr(aut))
rospy.wait_for_message(pose_topic_name, PoseStamped)
busy_waiting_start = rospy.Time.now()
start_time = busy_waiting_start.to_sec()
rospy.logdebug("Start %s at %.2f" % (aut, busy_waiting_start.to_sec()))
while not stop_ev.is_set() and not aut.reached_sink_state():
sleep(0.0) # Yield to other threads
# TODO avoid each iteration of while loop running indefinitely long
# Select an enabled action
act = _select_act(aut, i_queue)
# No enabled action, stay at current discrete state
# NOTE clock and continuous variables are still updated
if act is not None:
busy_waiting_start = rospy.Time.now()
else:
timeout = 300
if busy_waiting_start + rospy.Duration(secs=timeout) < rospy.Time.now():
print("Busy waiting for over %d seconds without new actions." % timeout, end=' ')
break
continue
# Send output action to the environment
if aut.is_output(act):
o_queue.put_nowait(act)
# Run transition of automaton
aut.transition(act)
except KeyboardInterrupt:
print("KeyboardInterrupt.", end=' ')
# except RuntimeError as e:
# print(repr(e), end=' ')
finally:
o_queue.close()
i_queue.close()
end_time = rospy.Time.now().to_sec()
rospy.logdebug("Ending %s at %.2f..." % (aut, end_time))
print('-', {"name": repr(aut), "t_start": start_time, "t_end": end_time, **aut.queries})
rospy.signal_shutdown("Shutting down ROS node for %s" % aut)
# * terms of the Eclipse Public License 2.0 which is available at
# * http://www.eclipse.org/legal/epl-2.0.
# *
# * SPDX-License-Identifier: EPL-2.0
# *
# * Contributors:
# * Daniel Heß
# ********************************************************************************
import math
import rospy
from adore_if_ros_msg.msg import NavigationGoal
from rospy.timer import sleep
if __name__ == '__main__':
rospy.init_node('adore_set_goal', anonymous=True)
output = 'ENV/NavigationGoal'
marker_name = rospy.get_param('~marker')
x0 = rospy.get_param(marker_name + '_x0', 0.0)
y0 = rospy.get_param(marker_name + '_y0', 0.0)
z0 = rospy.get_param(marker_name + '_z0', 0.0)
pub = rospy.Publisher(output, NavigationGoal, queue_size=1)
g = NavigationGoal()
g.target.x = x0
g.target.y = y0
g.target.z = z0
while pub.get_num_connections() < 2:
sleep(0.1)
for i in range(10):
pub.publish(g)
sleep(1.0)
rospy.spin()
def read_topic(self, my_topic, device):
device = device.lower()
print 'my_topic = ' + my_topic.__str__()
time_diff = rospy.Duration(1)
time_previous_message = None
# FIXME: The current timing code will probably have an increasing delay as messages are played.
# We should fix this by basing the sleep duration on the difference between
# the current wall clock and the desired time of the message
print('\nmsg count = ' + self.bag.get_message_count().__str__())
bag_messages = self.bag.read_messages(topics=[my_topic])
for topic, msg, time_current_message in bag_messages:
if device == "hardware":
if self.read_count == 0:
self.set_state(topic, 'DVRK_POSITION_GOAL_JOINT')
elif self.read_count == 3:
self.set_state(topic, 'DVRK_POSITION_JOINT')
self.read_count = self.read_count + 1
# print(self.read_count)
# the timestamp of each recorded message is saved as time_current_message
if time_previous_message != None:
time_diff = time_current_message - time_previous_message
temp_t = time.time()
sleep(time_diff)
# s = raw_input('\nPress any key to continue...(except q)')
# if s == 'q':
# return
print('time.time says {}\n'.format(time.time() - temp_t))
print('time_diff = {} '.format(time_diff.to_sec()))
# print('time_previous_message = {}, rostime = {}'.format(time_previous_message, rospy.rostime.get_rostime()))
time_previous_message = time_current_message
if 'PSM1' in topic or 'PSM2' in topic:
msg.name = [
'outer_yaw', 'outer_pitch', 'outer_insertion',
'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw'
]
elif 'ECM' in topic:
msg.name = [
'outer_yaw', 'outer_pitch', 'insertion', 'outer_roll'
]
elif 'MTM' in topic:
msg.name = [
'outer_yaw', 'shoulder_pitch', 'shoulder_pitch_parallel',
'elbow_pitch', 'wrist_platform', 'wrist_pitch',
'wrist_yaw', 'wrist_roll'
]
# publish
if self.read_count < 3 and device == "hardware":
print('HOMING..\n')
if self.arm_name in topic:
print(self.arm_name + topic)
rospy.Publisher('/dvrk/{}/set_position_goal_joint'.format(
self.arm_name),
JointState,
queue_size=10).publish(msg)
sleep(2)
else:
if device == "simulation":
self.pub_sim[topic].publish(msg)
if device == "hardware":
msg.position = [round(i, 4) for i in msg.position]
self.pub_hw[topic].publish(msg)
# * Daniel Heß
# ********************************************************************************
import math
import rospy
from geometry_msgs.msg import Pose
from rospy.timer import sleep
if __name__ == '__main__':
rospy.init_node('adore_set_pose', anonymous=True)
output = 'SIM/ResetVehiclePose'
marker_name = rospy.get_param('~marker')
x0 = rospy.get_param(marker_name + '_x0', 0.0)
y0 = rospy.get_param(marker_name + '_y0', 0.0)
z0 = rospy.get_param(marker_name + '_z0', 0.0)
x1 = rospy.get_param(marker_name + '_x1', 1.0)
y1 = rospy.get_param(marker_name + '_y1', 0.0)
z1 = rospy.get_param(marker_name + '_z1', 0.0)
pub = rospy.Publisher(output, Pose, queue_size=1)
p0 = Pose()
p0.position.x = x0
p0.position.y = y0
p0.position.z = z0
yaw = math.atan2(y1 - y0, x1 - x0)
p0.orientation.x = 0.0
p0.orientation.y = 0.0
p0.orientation.z = math.sin(yaw * 0.5)
p0.orientation.w = math.cos(yaw * 0.5)
while pub.get_num_connections() == 0:
sleep(0.1)
pub.publish(p0)
rospy.spin()
#random = 0 #time = rospy.get_rostime() newtime = time print "beginning loop" #while(key != '-' and key != '\x03'): while(time < 80 and key != '\x03'): # print msg if (key == '\x03'): break else: print "turn "+i twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = turn pub.publish(twist) timer.sleep(.8) time = time +1 #newtime = rospy.get_rostime() print "loop ended" twist = Twist() twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0 pub.publish(twist) print "Finished first round" if (key == '\x03'): break else: key = ' '
