def find_node(wanted_node_name, unique=False):
'''
Do a lookup to find a node with the given name. The given name is treated as the unresolved node name.
Hence this lookup will find nodes with the same name, but different namespaces.
This will raise exceptions, if the node couldn't be found
or in case unique is set multiple nodes with the same name are found.
:param str wanted_node_name: unresolved name of the node looked for (e.g. 'gateway', not '/concert/gateway')
:returns: the fully resolved name of the node (unique) or list of fully resolved names (non-unique)
:rtype: str
:raises: :exc:`.NotFoundException`
:todo: accept resolved names -> https://github.com/robotics-in-concert/rocon_tools/issues/30
'''
available_nodes = rosnode.get_node_names()
found_nodes = []
for resolved_node_name in available_nodes:
node_name = resolved_node_name[(resolved_node_name.rfind('/') + 1):len(resolved_node_name)]
if node_name == wanted_node_name:
found_nodes.append(resolved_node_name)
if len(found_nodes) == 0:
raise NotFoundException("Node '" + str(wanted_node_name) + "' not found.")
if unique:
if len(found_nodes) > 1:
raise NotFoundException('More then one node with the same name found:' + str(found_nodes))
return found_nodes
def is_node_up(node):
try:
node_up = any([node in upnode for upnode in
rosnode.get_node_names(namespace)])
return node_up
except Exception:
return False
def say(robot, msg, callback = None, feedback =None):
""" Says loudly the message.
Several TTS systems are tested:
- first, try the Acapela TTS (through the acapela-ros Genom module)
- then the ROS 'sound_play' node
- eventually, the Genom 'textos' module
:param msg: a text to say.
"""
def execute(robot):
logger.info("Robot says: " + msg)
if robot.supports(ROS):
import roslib; roslib.load_manifest('sound_play')
import rospy, os, sys
from sound_play.libsoundplay import SoundClient
soundhandle = SoundClient()
soundhandle.say(msg)
return (True, None)
elif robot.hasmodule("textos"):
return robot.execute([
genom_request(
"textos",
"Say",
[msg],
wait_for_completion = False if callback else True,
callback = callback)])
else:
logger.warning("No ROS, no textos module: can not do speech synthesis.")
return (True, None)
if robot.supports(ROS):
import rosnode
nodes = rosnode.get_node_names()
if "/acapela" in nodes:
import actionlib
from acapela.msg import SayGoal, SayAction
# use Acapela TTS
client = actionlib.SimpleActionClient('/acapela/Say', SayAction)
ok = client.wait_for_server()
if not ok:
print("Could not connect to the Acapela ROS action server! Aborting action")
return
# Creates a goal to send to the action server.
goal = SayGoal()
goal.message = msg
return [ros_request(client,
goal,
wait_for_completion = False if callback else True,
callback = callback,
feedback=feedback
)] # Return a non-blocking action. Useful to be able to cancel it later!
return [python_request(execute)]
def get_all_node_info(self):
infos = []
self.remove_dead_nodes()
for node_name in rosnode.get_node_names():
info = self.get_node_info(node_name)
if info is not False: infos.append((node_name, info))
return infos
def main(argv):
print "REFLEX GRIPPER RR BRIDGE"
print "========================"
# Parse command line arguments
parser = argparse.ArgumentParser(
description='Initialize a Gripper Hand')
parser.add_argument('--port', type=int, default=0,
help='TCP port to host service on' +\
'(will auto-generate if not specified)')
parser.add_argument('name', help='The desired name for the gripper.' +\
'Must match name given to start gripper services(e.g. left)')
args = parser.parse_args(argv)
# Check for valid name
name = args.name
if not "/reflex_%s"%(name) in rosnode.get_node_names():
print "The name '%s' is not valid. The gripper services may not "%(name) +\
"be started yet."
raw_input("Press ENTER to close")
return -1
# Enable numpy
RR.RobotRaconteurNode.s.UseNumPy = True
# Set the node name
RR.RobotRaconteurNode.s.NodeName = "ReflexGripperServer.%s"%name
# Initialize the object
gripper_obj = Gripper_impl(name)
# Create transport, register it, and start the server
print "Registering Transport..."
t = RR.TcpTransport()
t.EnableNodeAnnounce(RR.IPNodeDiscoveryFlags_NODE_LOCAL |
RR.IPNodeDiscoveryFlags_LINK_LOCAL |
RR.IPNodeDiscoveryFlags_SITE_LOCAL)
RR.RobotRaconteurNode.s.RegisterTransport(t)
t.StartServer(args.port)
port = args.port
if (port == 0):
port = t.GetListenPort()
# Register the service type and the service
print "Starting Service..."
RR.RobotRaconteurNode.s.RegisterServiceType(gripper_servicedef)
RR.RobotRaconteurNode.s.RegisterService("Gripper",
"ReflexGripper_Interface.Gripper",
gripper_obj)
print "Service started, connect via"
print "tcp://localhost:%s/ReflexGripperServer.%s/Gripper"%(port, name)
raw_input("press ENTER to quit ... \r\n")
gripper_obj.close()
# This must be here to prevent segfault
RR.RobotRaconteurNode.s.Shutdown()
def is_running(self):
if self._subprocess is not None:
return True
try:
return True if '/rosbridge_websocket' in rosnode.get_node_names(
) else False
except:
return False
def get_node_names(self):
"""
Gets a list of available services via a ros service call.
:returns: a list of all nodes that provide the set_logger_level service, ''list(str)''
"""
set_logger_level_nodes = []
nodes = rosnode.get_node_names()
for name in sorted(nodes):
for service in rosservice.get_service_list(name):
if service == name + '/set_logger_level':
set_logger_level_nodes.append(name)
return set_logger_level_nodes
def test_long_viseme(self):
filename = get_rosbag_file("long_viseme")
# job = play_rosbag(filename)
# job.join()
pub, msg_class = rostopic.create_publisher("/blender_api/queue_viseme", "blender_api_msgs/Viseme", True)
bag = rosbag.Bag(filename)
duration = bag.get_end_time() - bag.get_start_time()
fps = bag.get_message_count() / float(duration)
wait = 1.0 / fps / 10 # 10 times faster than the speed of the msg recoreded
for topic, msg, _ in rosbag_msg_generator(filename):
pub.publish(msg)
time.sleep(wait)
# Test if blender is still alive
self.assertIn("/blender_api", rosnode.get_node_names())
self.assertTrue(any(["blender_api" in name for name in self.runner.pm.get_active_names()]))
def get_node_info(self, event):
"""
Get the list of all Nodes running on the host.
Update the __node_list
"""
if not self.__is_enabled:
pass
if self.__is_enabled:
nodes = []
for node_name in rosnode.get_node_names():
try:
node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=True)
if self._id in node_api[2]:
nodes.append(node_name)
except :
pass
for node in nodes:
if node not in self.__node_list:
"""TODO currently not catching the exception here - master not running is a hard error so it does
not make sense to continue running.."""
node_api = rosnode.get_api_uri(rospy.get_master(), node, skip_cache=True)
try:
pid = self.get_node_pid(node_api, node)
if not pid:
continue
node_process = psutil.Process(pid)
new_node = NodeStatisticsHandler(
self._id, node, node_process)
self.__dict_lock.acquire(True)
self.__node_list[node] = new_node
self.__dict_lock.release()
except psutil.NoSuchProcess:
rospy.loginfo('pid of node %s could not be fetched' % node)
continue
self.__dict_lock.acquire()
to_remove = []
for node_name in self.__node_list:
if node_name not in nodes:
rospy.logdebug('removing %s from host' % node_name)
to_remove.append(node_name)
for node_name in to_remove:
self.remove_node(node_name)
self.__dict_lock.release()
def check_registration(e):
"""\
Shuts down this ROS node if it is not registered on the master.
This will effectively kill the director each time the ROS master is
restarted, preventing silent and subtle publishing failure.
This should cause a shutdown *only* if the master can be contacted and the
node is not registered.
"""
import rosnode
try:
nodes = rosnode.get_node_names()
except rosnode.ROSNodeIOException:
rospy.logdebug("Could not contact master for registration check")
return
if rospy.get_name() not in nodes:
rospy.logwarn("Node no longer registered, shutting down")
rospy.signal_shutdown("Node no longer registered")
os.kill(os.getpid(), signal.SIGTERM)
def checkNodeState(target_node_name, needed, sub_success="", sub_fail=""):
nodes = rosnode.get_node_names()
if target_node_name in nodes:
if needed:
okMessage("Node " + target_node_name + " exists")
if sub_success:
print Fore.GREEN + " " + sub_success + Fore.RESET
else:
errorMessage("Node " + target_node_name + " exists unexpecetedly. This should be killed with rosnode kill")
if sub_fail:
print Fore.RED + " " + sub_fail + Fore.RESET
else:
if needed:
errorMessage("Node " + target_node_name + " doesn't exists. This node is NEEDED")
if sub_fail:
print Fore.RED + " " + sub_fail + Fore.RESET
else:
okMessage("Node " + target_node_name + " doesn't exists")
if sub_success:
print Fore.GREEN + " " + sub_success + Fore.RESET
def test_rosnode_list(self):
import rosnode
cmd = 'rosnode'
nodes = ['/talker',
'/foo/talker',
'/bar/talker',
'/baz/talker1',
'/baz/talker2',
'/baz/talker3',
'/rosout',
]
l = rosnode.get_node_names()
for t in nodes:
self.assert_(t in l)
try:
rosnode._rosnode_cmd_list([cmd, 'list', 'one', 'two'])
self.fail("should have failed")
except SystemExit, e:
self.assertNotEquals(0, e.code)
def handle_pose(self,msg):
#print "MESSAGE", dir(msg)
pos = msg.pose.pose.position
if self.get_home:
if self.copter_name == rosnode.get_node_names()[0]:
GLOB_X = pos.x
GLOB_Y = pos.y
self.home_x = GLOB_X #pos.x
self.home_y = GLOB_Y #pos.y
self.get_home = False
q = msg.pose.pose.orientation
ang = q.x
#print pos.x, self.home_x
#print "X ",q.x, " Y ", q.y, " Z ", q.z, " W ", q.w
br.sendTransform(( pos.x-self.home_x, pos.y-self.home_y, pos.z),
(q.x, q.y, q.z, q.w),
rospy.Time.now(),
self.copter_name,
"fcu")
def getROSNodes(self):
nodes = []
try:
nodenames = rosnode.get_node_names(None)
except Exception as inst:
print "ERROR:[getROSNodes-01] ", inst
return nodes
for nodename in nodenames:
#rosnode.rosnode_info(nodename)
api = rosnode.get_api_uri(rosgraph.Master("/rosnode"), nodename)
if api:
try:
node = ServerProxy(api)
code, msg, pid = node.getPid("/rosnode")
if code == 1:
res = re.search("^(.*)_[0-9]+$", nodename)
while res is not None:
nodename = res.group(1)
res = re.search("^(.*)_[0-9]+$", nodename)
nodes.append((nodename, pid))
except:
pass
return nodes
def __init__(self, parent):
super(NodeSelection, self).__init__()
self.parent_widget = parent
self.selected_nodes = []
self.setWindowTitle("Select the nodes you want to record")
self.resize(500, 700)
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Done", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.node_list = rosnode.get_node_names()
self.node_list.sort()
for node in self.node_list:
self.addCheckBox(node)
self.main_widget.setLayout(self.selection_vlayout)
self.show()
def __init__(self):
#clean up previous process
os.system("killall gzserver gzclient")
if rosgraph.is_master_online(
): # Checks the master uri and results boolean (True or False)
print 'ROS MASTER is active'
nodes = rosnode.get_node_names()
if "/rviz" in nodes:
print("Rviz active")
rvizflag = " rviz:=false"
else:
rvizflag = " rviz:=true"
#start ros impedance controller
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
self.launch = roslaunch.parent.ROSLaunchParent(uuid, [
os.environ['LOCOSIM_DIR'] +
"/ros_impedance_controller/launch/ros_impedance_controller.launch"
])
#only available in ros lunar
# roslaunch_args=rvizflag
# self.launch = roslaunch.parent.ROSLaunchParent(uuid, [os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller_stdalone.launch"],roslaunch_args=[roslaunch_args])
self.launch.start()
ros.sleep(4.0)
threading.Thread.__init__(self)
# instantiate graphic utils
self.ros_pub = RosPub(True)
self.joint_names = ""
self.u = Utils()
self.comPoseW = np.zeros(6)
self.baseTwistW = np.zeros(6)
self.stance_legs = np.array([True, True, True, True])
self.q = np.zeros(12)
self.qd = np.zeros(12)
self.tau = np.zeros(12)
self.q_des = np.zeros(12)
self.qd_des = np.zeros(12)
self.tau_ffwd = np.zeros(12)
self.b_R_w = np.eye(3)
self.grForcesW = np.zeros(12)
self.basePoseW = np.zeros(6)
self.J = [np.eye(3)] * 4
self.wJ = [np.eye(3)] * 4
self.robot_name = ros.get_param('/robot_name')
self.sub_contact = ros.Subscriber("/" + self.robot_name +
"/contacts_state",
ContactsState,
callback=self._receive_contact,
queue_size=100)
self.sub_pose = ros.Subscriber("/" + self.robot_name + "/ground_truth",
Odometry,
callback=self._receive_pose,
queue_size=1)
self.sub_jstate = ros.Subscriber("/" + self.robot_name +
"/joint_states",
JointState,
callback=self._receive_jstate,
queue_size=1)
self.pub_des_jstate = ros.Publisher("/command",
JointState,
queue_size=1)
# freeze base and pause simulation service
self.reset_world = ros.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.reset_gravity = ros.ServiceProxy('/gazebo/set_physics_properties',
SetPhysicsProperties)
self.pause_physics_client = ros.ServiceProxy('/gazebo/pause_physics',
Empty)
self.unpause_physics_client = ros.ServiceProxy(
'/gazebo/unpause_physics', Empty)
# Loading a robot model of HyQ (Pinocchio)
self.robot = getRobotModel("hyq")
#send data to param server
self.verbose = conf.verbose
self.u.putIntoGlobalParamServer("verbose", self.verbose)
self.ros_notificator.shutdown()
except Exception, e:
logger.error("The ROS notificator could not be shut down")
logger.exception(e)
SimUtil.delete_simulation_dir()
# Cleanup ROS core nodes, services, and topics (the command should be almost
# instant and exit, but wrap it in a timeout since it's semi-officially supported)
logger.info("Cleaning up ROS nodes and services")
for param in rospy.get_param_names():
if param not in self._initial_ros_params:
rospy.delete_param(param)
for node in rosnode.get_node_names():
if node not in self._initial_ros_nodes:
os.system('rosnode kill ' + str(node))
try:
res = subprocess.check_output(["rosnode", "list"])
if res.find("/gazebo") > -1 and res.find("/Watchdog") > -1:
os.system('rosnode kill /gazebo /Watchdog')
elif res.find("/gazebo") > -1:
os.system('rosnode kill /gazebo >/dev/null 2>&1')
elif res.find("/Watchdog") > -1:
os.system('rosnode kill /Watchdog >/dev/null 2>&1')
except Exception, e:
def test_node_exist(self):
nodes = rosnode.get_node_names()
self.assertIn('/motors', nodes, "node does not exist")
def test_rosnode_list(self):
import rosnode
cmd = 'rosnode'
nodes = ['/talker',
'/foo/talker',
'/bar/talker',
'/baz/talker1',
'/baz/talker2',
'/baz/talker3',
'/rosout',
]
l = rosnode.get_node_names()
for t in nodes:
self.assert_(t in l)
try:
rosnode._rosnode_cmd_list([cmd, 'list', 'one', 'two'])
self.fail("should have failed")
except SystemExit as e:
self.assertNotEquals(0, e.code)
with fakestdout() as b:
rosnode._rosnode_cmd_list([cmd, 'list'])
self._check(nodes, tolist(b))
with fakestdout() as b:
rosnode._rosnode_cmd_list([cmd, 'list', '/'])
l = tolist(b)
self._check(nodes, l)
num_nodes = len(l)
# test -u uris
with fakestdout() as b:
rosnode._rosnode_cmd_list([cmd, 'list', '-u', '/'])
l = tolist(b)
self.assertEquals(num_nodes, len(l))
self.failIf([n for n in l if not n.startswith('http://')])
# test -a all
with fakestdout() as b:
rosnode._rosnode_cmd_list([cmd, 'list', '-a', '/'])
l = tolist(b)
uris = [x.split()[0] for x in l if x]
names = [x.split()[1] for x in l if x]
self._check(nodes, names)
self.assertEquals(num_nodes, len(uris))
self.failIf([n for n in uris if not n.startswith('http://')])
# test with namespace
foon = [p for p in nodes if p.startswith('/foo/')]
not_foon = [p for p in nodes if not p.startswith('/foo/')]
for ns in ['foo', '/foo', '/foo/']:
with fakestdout() as b:
rosnode._rosnode_cmd_list([cmd, 'list', ns])
self._check(foon, tolist(b))
self._notcheck(not_foon, tolist(b))
bazn = [p for p in nodes if p.startswith('/baz/')]
not_bazn = [p for p in nodes if not p.startswith('/baz/')]
for ns in ['baz', '/baz', '/baz/']:
with fakestdout() as b:
rosnode._rosnode_cmd_list([cmd, 'list', ns])
self._check(bazn, tolist(b))
self._notcheck(not_bazn, tolist(b))
# test with no match
with fakestdout() as b:
rosnode._rosnode_cmd_list([cmd, 'list', '/not/a/namespace/'])
self.assertEquals([], tolist(b))
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
def move_max_velocity(value=0.5):
arm.set_max_velocity_scaling_factor(value)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
###
### b-group
###
#紙Aコップのx座標 紙Aコップのy座標 紙Bコップx座標 紙Bコップy座標
position_base = [[0.30, 0.13, 0.30, -0.11]]
Acup_tukamu = False
Bcup_tukamu = False
"""
使い方
ハンドをつかんだ時は
position_manager(False,False,x,y,True)
ハンドをはなした時は
position_manager(False,False,x,y,False)
Aカップをつかみたい時の手先の位置を知りたいときは
aa = position_manager(True,True,0,0,False)
x = aa[0]
y = aa[1]
BBカップをつかみたい時の手先の位置を知りたいときは
aa = position_manager(True,False,0,0,False)
x = aa[0]
y = aa[1]
"""
def position_manager(master_judge, paper_cup, x, y, tukami):
global Acup_tukamu
global Bcup_tukamu
position_ret = [0.0, 0.0]
if master_judge == True:
if len(position_base) > 0:
#ホンスワン
if paper_cup == True:
position_ret[0] = position_base[len(position_base) - 1][2]
position_ret[1] = position_base[len(position_base) - 1][3]
#position_historyの末尾の紙Aコップのx座標y座標
return position_ret
else:
position_ret[0] = position_base[len(position_base) - 1][0]
position_ret[1] = position_base[len(position_base) - 1][1]
#position_historyの末尾の紙Bコップのx座標y座標
return position_ret
else:
return position_ret
else:
#熊谷さん
#position_base配列の末尾にx,yを追加
if tukami == True:
#Aをつかんでいる場合
if position_base[len(position_base) -
1][0] == x and position_base[
len(position_base) - 1][1] == y:
position_base.append([
x, y, position_base[len(position_base) - 1][2],
position_base[len(position_base) - 1][3]
])
Acup_tukamu = True
#Bをつかんでいる場合
elif position_base[len(position_base) -
1][2] == x and position_base[
len(position_base) - 1][3] == y:
position_base.append([
position_base[len(position_base) - 1][0],
position_base[len(position_base) - 1][1], x, y
])
Bcup_tukamu = True
else:
#Aをはなした時
if Acup_tukamu == True:
position_base.append([
x, y, position_base[len(position_base) - 1][2],
position_base[len(position_base) - 1][3]
])
Acup_tukamu = False
elif Bcup_tukamu == True:
position_base.append([
position_base[len(position_base) - 1][0],
position_base[len(position_base) - 1][1], x, y
])
Bcup_tukamu = False
return position_ret
###
### b-group-end
###
###
### a-group
###
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# ハンドを開く/ 閉じる
def move_gripper(pou):
gripper.set_joint_value_target([pou, pou])
gripper.go()
# アームを移動する
def move_arm(pos_x, pos_y, pos_z):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = pos_z
q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ上部をつかむ位置へ移動１
def move_arm_upper(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x - 0.13
target_pose.position.y = pos_y
target_pose.position.z = 0.075
q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ上部をつかむ位置へ移動２
def move_arm_upper_catch(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x - 0.005
target_pose.position.y = pos_y
target_pose.position.z = 0.075
q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ下部をつかむ位置へ移動１
def move_arm_lower(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x - 0.13
target_pose.position.y = pos_y
target_pose.position.z = -0.01
q = quaternion_from_euler(-3.14 / 2.0 - 0.2, 0.0,
-3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ下部をつかむ位置へ移動２
def move_arm_lower_catch(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x - 0.005
target_pose.position.y = pos_y
target_pose.position.z = -0.01
q = quaternion_from_euler(-3.14 / 2.0 - 0.2, 0.0,
-3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップの上を持ち上へ移動
def move_arm_upper_up(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x - 0.005
target_pose.position.y = pos_y
target_pose.position.z = 0.075 + 0.1
q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップの下を持ち上へ移動
def move_arm_lower_up(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x - 0.005
target_pose.position.y = pos_y
target_pose.position.z = -0.01 + 0.1
q = quaternion_from_euler(-3.14 / 2.0, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
###
### a-group-end
###
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
#ハンドを開く
move_gripper(1.3)
#コップA下部を掴む-----1
#流れ 1,2,3,4,5,6,7,8,9
#aaにコップAの(x,y)を代入
aa = position_manager(True, True, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップの下部と水平な部分へ移動
move_arm_lower(aa[0], aa[1])
#アームをコップの下部とくっつける
move_arm_lower_catch(aa[0], aa[1])
#グリッパを閉じ、コップをつかむ
move_gripper(0.28)
#アームを持ち上げる(空中で停止させるため複数回呼び出している)
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
#アームを下げる
move_arm_lower_catch(aa[0], aa[1])
#グリッパを開き、コップを放す
move_gripper(1.3)
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_lower_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
#コップB上部を掴む-----2
#流れ 10,11,12,13,14,15,16,17,18,19,20
#aaにコップBの(x,y)を代入
aa = position_manager(True, False, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップの下部と水平な部分へ移動
move_arm_upper(aa[0], aa[1])
#アームをコップの下部とくっつける
move_arm_upper_catch(aa[0], aa[1])
#グリッパを閉じ、コップをつかむ
move_gripper(0.28)
move_arm_upper_up(aa[0], aa[1])
#アームを持ち上げる(空中で停止させるため複数回呼び出している)
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
#アームをボールの真上へ(空中で停止させるため複数回呼び出している)
move_arm_upper_up(aa[0], aa[1] + 0.1)
move_arm_upper_up(aa[0], aa[1] + 0.1)
move_arm_upper_up(aa[0], aa[1] + 0.1)
move_arm_upper_up(aa[0], aa[1] + 0.1)
#アームを下げカップをボールにかぶせる
move_arm_upper_catch(aa[0], aa[1] + 0.1)
#元の位置へ戻す
move_arm_upper_catch(aa[0], aa[1])
#グリッパを開き、コップを放す
move_gripper(1.3)
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_upper_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
#パフォーマンスするならここ
#取り敢えず先輩のコードをそのまま流用
#頂点に振り上げる
move_arm(0.1, 0.0, 0.5)
#あおり
move_arm(0.22, -0.2, 0.2)
#頂点に振り上げる
move_arm(0.1, 0.0, 0.5)
#あおり
move_arm(0.22, 0.2, 0.2)
#homeに戻る
arm.set_named_target("home")
arm.go()
#コップB下部を掴む-----4
#瞬間移動 1,2,3,4,5,6,7,8,9
#aaにコップBの(x,y)を代入
aa = position_manager(True, False, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップの下部と水平な部分へ移動
move_arm_lower(aa[0], aa[1])
#アームをコップの下部とくっつける
move_arm_lower_catch(aa[0], aa[1])
#グリッパを閉じ、コップをつかむ
move_gripper(0.28)
#アームを持ち上げる(空中で停止させるため複数回呼び出している)
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
#アームを下げる
move_arm_lower_catch(aa[0], aa[1])
#グリッパを開き、コップを放す
move_gripper(1.3)
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_lower_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
#コップA上部を掴む-----4
#瞬間移動 10,11,12,13,14,15,16,17
#aaにコップAの(x,y)を代入
aa = position_manager(True, True, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップの上部と水平な部分へ移動
move_arm_upper(aa[0], aa[1])
#アームをコップの上部とくっつける
move_arm_upper_catch(aa[0], aa[1])
#グリッパを閉じ、コップをつかむ
move_gripper(0.28)
move_arm_upper_up(aa[0], aa[1])
#アームを持ち上げる(空中で停止させるため複数回呼び出している)
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
#アームを下げる
move_arm_upper_catch(aa[0], aa[1])
#グリッパを開き、コップを放す
move_gripper(1.3)
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_upper_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
#コップAの上部をつかむ-----5
#中身の確認
#aaにコップAの(x,y)を代入
aa = position_manager(True, True, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップの上部と水平な部分へ移動
move_arm_upper(aa[0], aa[1])
#アームをコップの上部とくっつける
move_arm_upper_catch(aa[0], aa[1])
#グリッパを閉じ、コップをつかむ
move_gripper(0.28)
move_arm_upper_up(aa[0], aa[1])
#アームを持ち上げる(空中で停止させるため複数回呼び出している)
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
move_arm_upper_up(aa[0], aa[1])
#アームを手前に移動
move_arm_upper_up(aa[0] - 0.13, aa[1])
#アームを下げる
move_arm_upper(aa[0], aa[1])
#グリッパを開き、コップを放す
move_gripper(1.3)
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_upper_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
#コップB下部を掴む-----6
#中身の確認
#aaにコップBの(x,y)を代入
aa = position_manager(True, False, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップの下部と水平な部分へ移動
move_arm_lower(aa[0], aa[1])
#アームをコップの下部とくっつける
move_arm_lower_catch(aa[0], aa[1])
#グリッパを閉じ、コップをつかむ
move_gripper(0.28)
#アームを持ち上げる(空中で停止させるため複数回呼び出している)
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
move_arm_lower_up(aa[0], aa[1])
#アームを手前に移動
move_arm_lower_up(aa[0] - 0.13, aa[1])
#アームを下げる
move_arm_lower(aa[0], aa[1])
#グリッパを開き、コップを放す
move_gripper(1.3)
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_lower_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
def add_any_pipeline_rois(self):
# TODO does the namespace argument do what i want?
ns = rospy.get_namespace()
nodes = rosnode.get_node_names(namespace=ns)
# TODO delete me
#rospy.logwarn('nodes w/ namespace=' + ns + ':' + str(nodes))
#rospy.logwarn('nodes w/o namespace specified:' + str(rosnode.get_node_names()))
roi_params = [['circular_mask_x', 'circular_mask_y', 'circular_mask_r'], \
['roi_points'], ['roi_l', 'roi_r', 'roi_b', 'roi_t']]
new_nodes = False
for n in nodes:
if 'tracker_' in n:
def getter(p):
return rospy.get_param(n + '/' + p)
else:
continue
node_num = int(n.split('_')[-1])
if not any(map(lambda d: node_num in d, [self.circular_rois, self.polygonal_rois, \
self.rectangular_rois])):
new_nodes = True
break
if not new_nodes:
return
self.clear_rois()
for n in nodes:
if 'tracker_' in n:
def getter(p):
return rospy.get_param(n + '/' + p)
else:
continue
for params in roi_params:
# may want to replace with rosnode API instrospection at some point
# provided it is stable / well documented enough
try:
# TODO need to prepend namespace?
# TODO TODO fix whatever causing roi_points get to be unreachable
# that is the one that is set, but it just tries for roi_l!
node_num = int(n.split('_')[-1])
self.add_roi(node_num, params, get_fn=getter)
# if the above roi is added successfully
# we will make it to this line, otherwise
# we will end up in the except block
self.start_subscribers(node_num)
except KeyError:
pass
if any(map(lambda x: len(x) > 0, [self.circular_rois, self.polygonal_rois, \
self.rectangular_rois])):
self.have_rois = True
else:
self.have_rois = False
# TODO TODO do some caching / checking to not have to recompute this everytime
self.image_mask = None
def onMessage(self, payload, isBinary):
# Debug
if isBinary:
print("Binary message received: {0} bytes".format(len(payload)))
else:
# print("Text message received: {0}".format(payload.decode('utf8')))
# Do stuff
# pub = rospy.Publisher('blockly', String, queue_size=10)
# time.sleep(1)
# pub.publish("blockly says: "+payload.decode('utf8'))
# Simple text protocol for communication
# first line is the name of the method
# next lines are body of message
message_text = payload.decode('utf8')
message_data = message_text.split('\n', 1)
if len(message_data) > 0:
method_name = message_data[0]
if len(message_data) > 1:
method_body = message_data[1]
if method_name.startswith('play'):
CodeStatus.set_current_status(CodeStatus.RUNNING)
BlocklyServerProtocol.build_ros_node(method_body)
rospy.loginfo('The program source file [' +
os.getcwd() + '/program.py] generated!')
# os.system('cat program.py')
if method_name == 'play2':
CodeExecution.run_process(['python', 'program.py'])
elif method_name == 'play3': # But py3 cannot be used
CodeExecution.run_process(
['python3', 'program.py'])
else:
rospy.logerr('Called unknown method %s', method_name)
else:
if 'pause' == method_name:
CodeStatus.set_current_status(CodeStatus.PAUSED)
elif 'resume' == method_name:
CodeStatus.set_current_status(CodeStatus.RUNNING)
elif 'end' == method_name:
#End program.py execution
global pid
print("@@@@@@@@@@@@@@@@@@")
try:
print("kill pid=" + str(pid))
os.kill(pid, signal.SIGKILL)
ros_nodes = rosnode.get_node_names()
except NameError:
print("execution script not running.")
pass
if '/imu_talker' in ros_nodes: #brain
##set default values
pub = rospy.Publisher('/statusleds',
String,
queue_size=10,
latch=True)
msg = 'blue_off'
pub.publish(msg)
msg = 'orange_off'
pub.publish(msg)
if '/crab_leg_kinematics' in ros_nodes: #spider
print("spider running")
pub = rospy.Publisher('/joy',
Joy,
queue_size=10,
latch=True)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range(0, 20):
msg.axes.append(valueAxe)
for e in range(0, 17):
msg.buttons.append(valueButton)
pub.publish(msg)
print("DEFAULT MESSAGES SENT")
if '/mavros' in ros_nodes: #rover
print("rover")
pub = rospy.Publisher('/mavros/rc/override',
OverrideRCIn,
queue_size=10)
msg = OverrideRCIn()
msg.channels[0] = 1500
msg.channels[1] = 0
msg.channels[2] = 1500
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
pub.publish(msg)
print("@@@@@@@@@@@@@@@@@@")
elif method_name.startswith('control'):
robot = method_name.split('control_')[1]
if robot.startswith('spider'):
direction = robot.split('spider_')[1]
pub = rospy.Publisher('/joy', Joy, queue_size=10)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range(0, 20):
msg.axes.append(valueAxe)
for e in range(0, 17):
msg.buttons.append(valueButton)
if direction == 'up': #forward
msg.axes[1] = 1
elif direction == 'down': #backwards
msg.axes[1] = -1
elif direction == 'left': #turn left
#msg.axes[0] = 1
msg.axes[2] = 1
elif direction == 'right': #turn rigth
#msg.axes[0] = -1
msg.axes[2] = -1
pub.publish(msg)
rate.sleep()
elif robot.startswith('rover'):
direction = robot.split('rover_')[1]
rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy(
'/mavros/set_mode', SetMode)
resp1 = change_mode(custom_mode='manual')
print(resp1)
if 'True' in str(resp1):
pub = rospy.Publisher('mavros/rc/override',
OverrideRCIn,
queue_size=10)
r = rospy.Rate(10) #2hz
msg = OverrideRCIn()
throttle_channel = 2
steer_channel = 0
speed_slow = 1558
#speed_turbo = 2000 #dangerous
speed = speed_slow
if direction == 'up': #forward
msg.channels[throttle_channel] = speed
msg.channels[
steer_channel] = 1385 #straight
elif direction == 'down': #backwards
msg.channels[
throttle_channel] = 1450 #slow
msg.channels[
steer_channel] = 1385 #straight
elif direction == 'left': #turn left
msg.channels[throttle_channel] = speed
msg.channels[steer_channel] = 1285
elif direction == 'right': #turn rigth
msg.channels[throttle_channel] = speed
msg.channels[steer_channel] = 1485
start = time.time()
flag = True
while not rospy.is_shutdown() and flag:
sample_time = time.time()
if ((sample_time - start) > 0.5):
flag = False
pub.publish(msg)
r.sleep()
else:
rospy.logerr('Called unknown method %s', method_name)
def remove_dead_nodes(self):
running_nodes = rosnode.get_node_names()
dead_nodes = [node_name for node_name in self.nodes if node_name not in running_nodes]
for node_name in dead_nodes:
self.nodes.pop(node_name, None)
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.1)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴む準備をする
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴みに行く
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.13
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを閉じる
gripper.set_joint_value_target([0.4, 0.4])
gripper.go()
# 持ち上げる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# 移動する
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.2
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# 下ろす
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.2
target_pose.position.z = 0.13
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 少しだけハンドを持ち上げる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.2
target_pose.position.z = 0.2
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
print("done")
def do_dump(self, line):
"""Dump information"""
nodes = rosnode.get_node_names()
rospy.loginfo("node list:%s" % (nodes))
def do_test(self, line):
"""Current test function"""
nodes = rosnode.get_node_names()
rospy.loginfo("node list:%s" % (nodes))
def __init__(self):
self.ekf_pub = rospy.Publisher('/ekf', Odometry, queue_size=10)
#%% Encoder variables
self.flag_encR = True
self.flag_encL = True
self.wl_ini = 0.0
self.wr_ini = 0.0
self.radio = 0.0725
self.enc_Ts = 0.0
self.enc_tic = 0.0
self.wr = 0.0
self.wl = 0.0
self.wr_1 = 0.0
self.wl_1 = 0.0
self.wr_2 = 0.0
self.wl_2 = 0.0
#%% Filter_encoders variables
self.acce = 20.0
self.wr_f_2 = 0.0
self.wr_f_1 = 0.0
self.wl_f_2 = 0.0
self.wl_f_1 = 0.0
#%% ICR estimation
self.ICR = 0.0
self.ICR_1 = 0.0
self.ICR_2 = 0.0
self.ICR_f1 = 0.0
self.ICR_f2 = 0.0
#%% IMU variables
self.flag_imu = True
self.euler1 = [0, 0, 0]
self.imu_toc = 0.0
self.imu_Ts = 0.0
self.imu_tic = 0.0
self.imu_pitch = 0.0
self.imu_roll = 0.0
self.imu_yaw = 0.0
#%% Mag variables
self.flag_mag = True
self.yaw_mag1 = 0.0
self.mag_toc = 0.0
self.mag_Ts = 0.0
self.mag_tic = 0.0
self.mag_yaw = 0.0
self.mag_yaw_1 = 0.0
self.Wz = 0.0
self.x_mag = 0.0
self.y_mag = 0.0
self.z_mag = 0.0
self.Wz_2 = 0.0
self.Wz_1 = 0.0
self.Wz_f2 = 0.0
self.Wz_f1 = 0.0
#%% Kalman variables
# Matrix error of covariance
self.kalman_P = np.matrix([[100.0, 0.0, 0.0], [0.0, 100.0, 0.0],
[0.0, 0.0, 100.0]])
# Covariance matrix of process noise
# self.kalman_Q = np.matrix([[1, 0.0, 0.0],[0.0, 1, 0.0],[0.0, 0.0, 0.001]])
self.kalman_Q = np.matrix([[
5.627197978234252e-07, -8.660344555066894e-08,
1.905427279718744e-06
],
[
-8.660344555066894e-08,
1.4065639626880477e-08,
-2.933464617904681e-07
],
[
1.905427279718744e-06,
-2.933464617904681e-07,
6.460098912945566e-06
]])
# Covariance matrix of measurement noise
self.kalman_R = 0.0001817647107593486
self.Yg = 1.2
self.X_hat = np.matrix([[0.0], [0.0], [0.0]])
#%% INIT
self.today = date.today()
self.time = rospy.Time.now()
print("Starting Kalman node - V2.5.T")
rospy.Subscriber('/WR', Float32, self.Wr_callbaback)
rospy.Subscriber('/WL', Float32, self.Wl_callbaback)
rospy.Subscriber('/imu/data', Imu, self.calIMU) #100 Hz
rospy.Subscriber('/imu/mag', MagneticField, self.calMag) #100 Hz
Flag_out = False
self.Flag_start = False
while not Flag_out:
node_names = rosnode.get_node_names()
# bag_node = [i for i in node_names if 'play' in i]
bag_node = [i for i in node_names if 'xsens' in i]
if (len(bag_node) == 0) and self.Flag_start:
rospy.signal_shutdown('finish')
Flag_out = True
else:
self.Kalman()
exit()
def onMessage(self, payload, isBinary):
# Debug
if isBinary:
print("Binary message received: {0} bytes".format(len(payload)))
else:
print("Text message received: {0}".format(payload.decode('utf8')))
# Do stuff
# pub = rospy.Publisher('blockly', String, queue_size=10)
# time.sleep(1)
# pub.publish("blockly says: "+payload.decode('utf8'))
# Simple text protocol for communication
# first line is the name of the method
# next lines are body of message
message_text = payload.decode('utf8')
message_data = message_text.split('\n', 1)
if len(message_data) > 0:
method_name = message_data[0]
if len(message_data) > 1:
method_body = message_data[1]
if 'play' == method_name:
CodeStatus.set_current_status(CodeStatus.RUNNING)
BlocklyServerProtocol.build_ros_node(method_body)
rospy.loginfo('The file generated contains...')
os.system('cat test.py')
CodeExecution.run_process(['python3', 'test.py'])
else:
rospy.logerr('Called unknown method %s', method_name)
else:
if 'pause' == method_name:
CodeStatus.set_current_status(CodeStatus.PAUSED)
elif 'resume' == method_name:
CodeStatus.set_current_status(CodeStatus.RUNNING)
elif 'end' == method_name:
#End test.py execution
global pid
print("@@@@@@@@@@@@@@@@@@")
try:
print("kill pid="+str(pid))
os.kill(pid, signal.SIGKILL)
ros_nodes = rosnode.get_node_names()
if '/imu_talker' in ros_nodes: #brain
##set default values
pub = rospy.Publisher('/statusleds', String, queue_size=10, latch=True)
msg = 'blue_off'
pub.publish(msg)
msg = 'orange_off'
pub.publish(msg)
if '/crab_leg_kinematics' in ros_nodes: #spider
print("spider running")
pub = rospy.Publisher('/joy', Joy, queue_size=10, latch=True)
msg = Joy()
msg.header.stamp = rospy.Time.now()
rate = rospy.Rate(10)
valueAxe = 0.0
valueButton = 0
for i in range (0, 20):
msg.axes.append(valueAxe)
for e in range (0, 17):
msg.buttons.append(valueButton)
pub.publish(msg)
rate.sleep()
print("DEFAULT MESSAGES SENT")
print("@@@@@@@@@@@@@@@@@@")
except NameError:
print("execution script not running.")
pass
else:
rospy.logerr('Called unknown method %s', method_name)
if __name__ == '__main__':
print('[INFO] Starting ros communication system...')
script_path = os.path.dirname(os.path.realpath(__file__))
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch_pcd = roslaunch.parent.ROSLaunchParent(uuid, [os.path.join(script_path, '..', 'launch', 'grasping_pcd_processing.launch')])
launch_pcd.start()
rospy.sleep(5)
capture_helper = CaptureHelper()
ROSSMartServo_on = False
while not ROSSMartServo_on:
rospy.loginfo('Please make sure you have started the ROSSMartServo application on the Sunrise Cabinet')
ans = raw_input(
'[USER INPUT] Type [y] and press [enter] if you have started the ROSSMartServo, otherwise exit the program: ')
if ans == 'y':
if '/iiwa/iiwa_subscriber' in rosnode.get_node_names():
ROSSMartServo_on = True
else:
rospy.loginfo('IIWA topics not detected, check network connection if you have started the SmartServo')
else:
rospy.loginfo('Exiting the program...')
launch_pcd.shutdown()
exit()
print('[INFO] Starting kuka controller...')
launch_controller = roslaunch.parent.ROSLaunchParent(uuid, [os.path.join(script_path, '..', 'launch', 'grasping_controller.launch')])
launch_controller.start()
rospy.sleep(5)
rospy.loginfo('Ros communication system has been properly setup')
while True:
ang = q.x
#print pos.x, self.home_x
#print "X ",q.x, " Y ", q.y, " Z ", q.z, " W ", q.w
br.sendTransform(( pos.x-self.home_x, pos.y-self.home_y, pos.z),
(q.x, q.y, q.z, q.w),
rospy.Time.now(),
self.copter_name,
"fcu")
if __name__ == '__main__':
rospy.init_node('px4_swarm_tfs_broadcaster')
br = tf.TransformBroadcaster()
rate = rospy.Rate(10.0)
for copter_name in rosnode.get_node_names():
if "mavros" in copter_name:
pb = publish_copter_frame(copter_name)
rospy.Subscriber(copter_name+'/global_position/local',
Odometry,
pb.handle_pose)
while not rospy.is_shutdown():
try:
pass
except KeyboardInterrupt:
rospy.signal_shutdown("Keyboard Interrupt")
rate.sleep()
rospy.spin()
def test_node_starts(self):
nodes = rosnode.get_node_names()
self.assertIn(node_name, nodes)
def get_nodes():
""" Returns a list of all the nodes registered in the ROS system """
return get_node_names()
def update_nodepkgmap(self):
master = roslib.scriptutil.get_master()
nodes = rosnode.get_node_names()
for n in nodes:
self.nodepkgmap[n] = self.getnodepkg1(n)[0]
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.1)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
#print("Group names:")
#print(robot.get_group_names())
#print("Current state:")
# print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
#print("Arm initial pose:")
#print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴む準備をする
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0
target_pose.position.y = -0.3
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴みに行く
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0
target_pose.position.y = -0.3
target_pose.position.z = 0.07
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを閉じる
gripper.set_joint_value_target([0.2, 0.2])
gripper.go()
# SRDFに定義されている"landing"の姿勢にする
arm.set_named_target("landing")
arm.go()
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# SRDFに定義されている"vertical"の姿勢にする
#アルコールに当たることがあるので一度アームを垂直にする。
arm.set_named_target("vertical")
arm.go()
#アームを開いて押す準備をする
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
#アームをアルコールの上に持ってくる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.1
target_pose.position.z = 0.33
q = quaternion_from_euler(-3.14, 0.0,
-0.7) #手首をz軸に80度ほど回転させた姿勢にしてボトルのノズルを押しやすくする。
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
#アルコールボトルを押す
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.1
target_pose.position.z = 0.21
q = quaternion_from_euler(-3.14, 0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
rospy.sleep(1.0)
#アルコールボトルからアームを引く
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.1
target_pose.position.z = 0.33
q = quaternion_from_euler(-3.14, 0.0, -0.7) #先程の手首をz軸に80度ほど回転させた姿勢にする。
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
#home(元々の)の姿勢に戻す。
arm.set_named_target("home")
arm.go()
print("done")
def test_node_exist(self):
nodes = rosnode.get_node_names()
self.assertIn('/buzzer',nodes, "node does not exist")
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
def move_max_velocity(value=0.5):
arm.set_max_velocity_scaling_factor(value)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
###
### b-group
###
#紙Aコップのx座標 紙Aコップのy座標 紙Bコップx座標 紙Bコップy座標
position_base = [[0.29, 0.13, 0.29, -0.11]]
Acup_tukamu = False
Bcup_tukamu = False
"""
使い方
ハンドをつかんだ時は
position_manager(False,False,x,y,True)
ハンドをはなした時は
position_manager(False,False,x,y,False)
Aカップをつかみたい時の手先の位置を知りたいときは
aa = position_manager(True,True,0,0,False)
x = aa[0]
y = aa[1]
BBカップをつかみたい時の手先の位置を知りたいときは
aa = position_manager(True,False,0,0,False)
x = aa[0]
y = aa[1]
"""
def position_manager(master_judge, paper_cup, x, y, tukami):
global Acup_tukamu
global Bcup_tukamu
position_ret = [0.0, 0.0]
if master_judge == True:
if len(position_base) > 0:
#ホンスワン
if paper_cup == True:
position_ret[0] = position_base[len(position_base)-1][2]
position_ret[1] = position_base[len(position_base)-1][3]
#position_historyの末尾の紙Aコップのx座標y座標
return position_ret
else:
position_ret[0] = position_base[len(position_base)-1][0]
position_ret[1] = position_base[len(position_base)-1][1]
#position_historyの末尾の紙Bコップのx座標y座標
return position_ret
else:
return position_ret
else:
#熊谷さん
#position_base配列の末尾にx,yを追加
if tukami == True:
#Aをつかんでいる場合
if position_base[len(position_base)-1][0] == x and position_base[len(position_base)-1][1] == y:
position_base.append([x, y, position_base[len(
position_base)-1][2], position_base[len(position_base)-1][3]])
Acup_tukamu = True
#Bをつかんでいる場合
elif position_base[len(position_base)-1][2] == x and position_base[len(position_base)-1][3] == y:
position_base.append([position_base[len(
position_base)-1][0], position_base[len(position_base)-1][1], x, y])
Bcup_tukamu = True
else:
#Aをはなした時
if Acup_tukamu == True:
position_base.append([x, y, position_base[len(
position_base)-1][2], position_base[len(position_base)-1][3]])
Acup_tukamu = False
elif Bcup_tukamu == True:
position_base.append([position_base[len(
position_base)-1][0], position_base[len(position_base)-1][1], x, y])
Bcup_tukamu = False
return position_ret
###
### b-group-end
###
###
### a-group
###
stop_time = 2.0 # 停止する時間を指定
weak_position = 0.13 # 上をつかむ際のz
weak_power = 0.24 # 上をつかむ際の力
strong_position = 0.11 # 下をつかむ際のｚ
strong_power = 0.28 # 下をつかむ際の力
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# ハンドを開く/ 閉じる
def move_gripper(pou):
gripper.set_joint_value_target([pou, pou])
gripper.go()
# アームを移動する
def move_arm(pos_x, pos_y, pos_z):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = pos_z
q = quaternion_from_euler(-3.14/2.0, 3.14, -3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ上部をつかむ位置へ移動１
def move_arm_upper(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = 0.25
q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ上部をつかむ位置へ移動２
def move_arm_upper_catch(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = weak_position
q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ下部をつかむ位置へ移動１
def move_arm_lower(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = 0.25
q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップ下部をつかむ位置へ移動２
def move_arm_lower_catch(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = strong_position
q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップの上を持ち上へ移動
def move_arm_upper_up(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = 0.25
q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# コップの下を持ち上へ移動
def move_arm_lower_up(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = 0.25
q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ボールをつかむ位置へ移動
def move_arm_ball(pos_x, pos_y):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = 0.1
q = quaternion_from_euler(0, 3.14, 0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
###
### a-group-end
###
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
#ハンドを開く
move_gripper(1.3)
#コップA-----1
#aaにコップAの(x,y)を代入
aa = position_manager(True, True, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップをくっつける
move_arm_upper_catch(aa[0], aa[1])
print("グリッパ下にボールを入れた紙コップを設置してください。")
rospy.sleep(stop_time)
print("次の動作へ進みます。")
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_lower_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
#コップB-----2
#aaにコップBの(x,y)を代入
aa = position_manager(True, False, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをコップの下部とくっつける
move_arm_upper_catch(aa[0], aa[1])
print("グリッパ下に空の紙コップを設置してください。")
rospy.sleep(stop_time)
print("次の動作へ進みます。")
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_upper_up(aa[0], aa[1])
move_max_velocity()
arm.set_named_target("home")
arm.go()
#ボール-----3
#aaにコップBの(x,y)を代入
aa = position_manager(True, False, 0, 0, False)
#グリッパを開く
move_gripper(1.3)
#アームをボールとくっつける
move_arm_ball(aa[0], aa[1] + 0.1)
print("グリッパ下にボールを設置してください。")
rospy.sleep(stop_time)
print("次の動作へ進みます。")
#アームを持ち上げコップから離れたらホームへ戻る
move_arm_ball(aa[0], aa[1] + 0.1)
move_max_velocity()
arm.set_named_target("home")
arm.go()
service_name = rospy.get_param('~service')
tries = 0
try:
service = rospy.ServiceProxy(service_name, std_srvs.srv.Empty)
rospy.loginfo("Waiting for service %s", service_name)
service.wait_for_service()
rospy.loginfo("Starting service, will wait for %f seconds", wait_time)
rate = rospy.Rate(spin_rate)
while not rospy.is_shutdown(): #rospy.Time.now() < end:
if trigger in rosnode.get_node_names():
rospy.loginfo("waiting for %f seconds now that %s is up", wait_time, trigger)
rospy.sleep(wait_time)
rospy.loginfo("trying to publish scene information...")
service()
tries = tries + 1
if tries > 5:
break
rate.sleep()
except rospy.ROSInterruptException: pass
def drone_viz():
rospy.init_node("viz")
drones = []
position_dict = {}
waypoints_dict = {}
waypointsbox_dict = {}
pub_dict = {}
rate = rospy.Rate(10)
pub = rospy.Publisher("viz/drone_markers", MarkerArray, queue_size=1)
markers = {}
waypoints = {}
waypointsbox = {}
trajectories = {}
marker_array = MarkerArray()
global_id = 0
BOX = "box"
while not rospy.is_shutdown():
#on fixed timestip: simulate the system and publish
nodes = rosnode.get_node_names()
for node in nodes:
node = node[1:]
if node.startswith("drone") and node not in drones or node == BOX:
drones.append(node)
rospy.Subscriber("/" + node + "/position", Pose, callback, (node, position_dict))
rospy.Subscriber("/" + node + "/waypoint", Float32MultiArray, callback, (node, waypoints_dict))
t, lifetime = rospy.Time.now(), rospy.Duration()
if BOX in drones and BOX in position_dict:
if BOX not in markers:
markers[BOX] = Marker()
marker = markers[BOX]
marker.header.frame_id = "world"
marker.id = global_id
global_id += 1
marker.header.stamp = t
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.scale = Vector3(0.1,0.1,0.1)
marker.color.r = 0.4
marker.color.g = 1
marker.color.b = 0.4
marker.color.a = 1.0
marker.lifetime = lifetime
marker_array.markers.append(marker)
marker = markers[BOX]
marker.pose = position_dict[BOX]
if BOX not in trajectories:
trajectories[BOX] = Marker()
marker = trajectories[BOX]
marker.header.frame_id = "world"
marker.id = global_id
global_id += 1
marker.header.stamp = t
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.scale = Vector3(0.01,0.01,0.01)
marker.color.r = 0.4
marker.color.g = 1
marker.color.b = 0.7
marker.color.a = 1.0
marker.lifetime = lifetime
marker_array.markers.append(marker)
marker = trajectories[BOX]
p = Point()
p.x = position_dict[BOX].position.x
p.y = position_dict[BOX].position.y
p.z = position_dict[BOX].position.z
marker.points.append(p)
# marker.points = marker.points[-400:]
for i, drone in enumerate(drones):
if drone == BOX: continue
if drone not in position_dict: continue
if drone not in markers:
markers[drone] = Marker()
marker = markers[drone]
marker.header.frame_id = "world"
marker.id = global_id
global_id += 1
marker.header.stamp = t
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://drone_viz/src/drone.stl"
#marker.type = Marker.CYLINDER
#marker.action = Marker.ADD
marker.scale = Vector3(0.001,0.001,0.001)
marker.color.r = 0.4
marker.color.g = 1
marker.color.b = 1
marker.color.a = 1.0
marker.lifetime = lifetime
marker_array.markers.append(marker)
marker = markers[drone]
marker.pose = position_dict[drone]
marker.pose = Pose()
marker.pose.position.x = position_dict[drone].position.x
marker.pose.position.y = position_dict[drone].position.y - 0.04
marker.pose.position.z = position_dict[drone].position.z
pos_msg = position_dict[drone]
euler = np.array(euler_from_quaternion([
pos_msg.orientation.x,
pos_msg.orientation.y,
pos_msg.orientation.z,
pos_msg.orientation.w
]))
euler[0] += -np.pi/2
quat = quaternion_from_euler(euler[0], euler[1], euler[2])
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
#waypoint
if drone not in waypoints_dict: continue
if drone not in waypoints:
waypoints[drone] = Marker()
marker = waypoints[drone]
marker.header.frame_id = "world"
marker.id = global_id
global_id += 1
marker.header.stamp = t
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.scale = Vector3(0.05,0.05,0.05)
marker.color.r = 1
marker.color.g = 0.4
marker.color.b = 0.4
marker.color.a = 1.0
marker.lifetime = lifetime
marker_array.markers.append(marker)
marker = waypoints[drone]
if len(marker.points) == 0 or marker.points[-1] != waypoints_dict[drone]:
p = Point()
p.x = waypoints_dict[drone].data[0]
p.y = waypoints_dict[drone].data[1] - 0.04
p.z = waypoints_dict[drone].data[2]
marker.points.append(p)
#trajectories
if drone not in trajectories:
trajectories[drone] = Marker()
marker = trajectories[drone]
marker.header.frame_id = "world"
marker.id = global_id
global_id += 1
marker.header.stamp = t
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.scale = Vector3(0.01,0.01,0.01)
marker.color.r = 0.4
marker.color.g = 0.7
marker.color.b = 1
marker.color.a = 1.0
marker.lifetime = lifetime
marker_array.markers.append(marker)
marker = trajectories[drone]
p = Point()
p.x = position_dict[drone].position.x
p.y = position_dict[drone].position.y - 0.04
p.z = position_dict[drone].position.z
marker.points.append(p)
# marker.points = marker.points[-400:]
if BOX in drones and BOX in position_dict:
# draw rope
marker_name = "{}-{}".format(BOX, drone)
if marker_name not in markers:
markers[marker_name] = Marker()
marker = markers[marker_name]
marker.header.frame_id = "world"
marker.id = global_id
global_id += 1
marker.header.stamp = t
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.scale = Vector3(0.008,0.008,0.008)
marker.color.r = 1
marker.color.g = 0.7
marker.color.b = 0.4
marker.color.a = 1.0
marker.lifetime = lifetime
marker_array.markers.append(marker)
marker = markers[marker_name]
drone_pos = position_dict[drone]
drone_point = Point(drone_pos.position.x, drone_pos.position.y, drone_pos.position.z - 0.01)
box_pos = position_dict[BOX]
box_point = Point(box_pos.position.x, box_pos.position.y, box_pos.position.z)
marker.points = [drone_point, box_point]
pub.publish(marker_array)
rate.sleep()
def cleanup(self, node_name):
master = rosgraph.Master(rosnode.ID)
nodes_to_cleanup = [n for n in rosnode.get_node_names() if n.startswith(node_name)]
rosnode.cleanup_master_blacklist(master, nodes_to_cleanup)
def test_running(self):
nodes = rosnode.get_node_names()
self.assertIn('/gazebo', nodes, 'node does not exit')
self.assertIn('/gmapping', nodes, 'node does not exit')
def main():
rospy.init_node("terminal_control")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.1)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
x_pos = 0.2
y_pos = 0.0
z_pos = 0.3
x_ang = -3.14
y_ang = 0.0
z_ang = -3.14 / 2.0
grip_ang = 0.7
while (True):
print("Arm current pose:")
print(arm.get_current_pose().pose)
input_mode = raw_input(
"Position (p), Orientation (o) or both (b)? Toggle Gripper (t). Otherwise, quit (q) "
)
if (input_mode is not "p" and input_mode is not "o"
and input_mode is not "b" and input_mode is not "q"
and input_mode is not "t"):
print("Try again")
continue
elif input_mode is "q":
print("Exiting command mode, shutting down...")
break
if input_mode is "p" or input_mode is "b":
while (True): #keep trying until get 3 valid points
try:
x_pos = float(
raw_input(
"Cartesian Coordinate (x) in metres from base: "))
y_pos = float(
raw_input(
"Cartesian Coordinate (y) in metres from base: "))
z_pos = float(
raw_input(
"Cartesian Coordinate (z) in metres from base: "))
break
except:
print("Enter valid float values, please")
pass
if input_mode is "o" or input_mode is "b":
while (True): #keep trying until get 3 valid points
try:
x_ang = float(raw_input("Angle about x-axis in radians: "))
y_ang = float(raw_input("Angle about y-axis in radians: "))
z_ang = float(raw_input("Angle about z-axis in radians: "))
break
except:
print("Enter valid float values, please")
pass
if (input_mode is not "t"):
# 掴む準備をする
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = x_pos #0.2
target_pose.position.y = y_pos #0.0
target_pose.position.z = z_pos #0.3
q = quaternion_from_euler(
x_ang, y_ang, z_ang) #(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
elif input_mode is "t":
# ハンドを開く
grip_ang = 0.7 if grip_ang < 0.7 else 0.3
gripper.set_joint_value_target([grip_ang, grip_ang]) #([0.7, 0.7])
gripper.go()
print("done")
print("shutting down the arm...")
#!/usr/bin/env python
from reflex_gripper.reflex_sf_hand import ReflexSFHand
from std_srvs.srv import Empty
import argparse, sys, rosnode, rospy
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Calibrate a Gripper Hand")
parser.add_argument('name', help="The name of the gripper hand to calibrate")
args = parser.parse_args(sys.argv[1:])
name = args.name
if '/reflex_%s'%name in rosnode.get_node_names():
print "If you do not see the script running in this window, "
print "please check the window that the hand service is running in."
rospy.wait_for_service('reflex_%s/zero_fingers'%name, 5)
service = rospy.ServiceProxy('/reflex_%s/zero_fingers'%name, Empty)
service()
else:
hand = ReflexSFHand(name)
hand.calibrate()
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
def move_max_velocity(value=0.5):
arm.set_max_velocity_scaling_factor(value)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
stop_time = 2.0 # 停止する時間を指定
force_hold_stick = 0.5 # 棒を握る力を指定
te_x_position_vertical = 0.193040 # x
te_y_position_vertical = -0.233386 # y
te_z_position_vertical = 0.085469 # z
stick_angle_vertical = 1.3 # 棒
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# ハンドを開く/ 閉じる
def move_gripper(pou):
gripper.set_joint_value_target([pou, pou])
gripper.go()
# アームを移動する
def move_arm(pos_x, pos_y, pos_z):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = pos_x
target_pose.position.y = pos_y
target_pose.position.z = pos_z
q = quaternion_from_euler(-3.14 / 2.5, 3.14, -3.14 / 2.0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
def preparation_vertical():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = te_x_position_vertical
target_pose.position.y = te_y_position_vertical
target_pose.position.z = te_z_position_vertical
q = quaternion_from_euler(-3.14 / 2.0, 3.14, -3.14 / 2.0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
def preparation2_vertical():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = te_x_position_vertical
target_pose.position.y = te_y_position_vertical
target_pose.position.z = te_z_position_vertical
q = quaternion_from_euler(-3.14 / 2.15, 3.14, -3.14 / 2.0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
def p1_vertical():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = te_x_position_vertical
target_pose.position.y = te_y_position_vertical
target_pose.position.z = te_z_position_vertical
q = quaternion_from_euler(0, 3.14, -0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
def p2_vertical():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = te_x_position_vertical
target_pose.position.y = te_y_position_vertical
target_pose.position.z = te_z_position_vertical
q = quaternion_from_euler(0, 3.14, -3.14 / 2.0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
def p3_vertical():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = te_x_position_vertical
target_pose.position.y = te_y_position_vertical
target_pose.position.z = te_z_position_vertical
q = quaternion_from_euler(1, 3.14, 0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
def p4_vertical():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = te_x_position_vertical
target_pose.position.y = te_y_position_vertical
target_pose.position.z = te_z_position_vertical
q = quaternion_from_euler(0, 3.14, -3.14 / 2.0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
arm.go()
preparation_vertical()
preparation_vertical()
p1_vertical()
preparation2_vertical()
p2_vertical()
preparation2_vertical()
move_max_velocity()
arm.set_named_target("home")
arm.go()
def commandCB(self, data):
if self.status == NOT_FLYING:
self.goal.yaw = utils.quat2yaw(self.pose.orientation)
if data.command == data.CMD_TAKEOFF and self.status == NOT_FLYING:
if '/rosbag_ft' in rosnode.get_node_names():
self.status = FLYING
self.wpType = TAKEOFF
# set initial goal to current pose
self.goal.pos = copy.copy(self.pose.position)
self.goal.vel.x = self.goal.vel.y = self.goal.vel.z = 0
self.goal.yaw = utils.quat2yaw(self.pose.orientation)
self.goal.dyaw = 0
rospy.loginfo("Flying")
else:
rospy.logerr('Can\'t takeoff without logging !! --Nick Roy')
elif data.command == data.CMD_HOVER and self.go == True:
rospy.loginfo("Entering state of hover")
self.stop_now = True
# emergency disable
elif data.command == data.CMD_KILL and self.status == FLYING:
self.status = NOT_FLYING
self.wpType = DISABLE
self.go = False
# landing
elif data.command == data.CMD_LAND and self.status == FLYING:
self.go = False
self.status = LANDING
# self.wpType = LAND
self.goal.pos.x = self.pose.position.x
self.goal.pos.y = self.pose.position.y
self.goal.vel.x = 0
self.goal.vel.y = 0
self.goal.vel.z = 0
self.goal.dyaw = 0
self.I = 0
elif data.command == data.CMD_GO and self.status == FLYING:
if self.received_coeff:
self.eval_splines()
self.goal.pos.x = self.x
self.goal.pos.y = self.y
self.goal.pos.z = self.z
self.goal.vel.x = 0
self.goal.vel.y = 0
self.goal.vel.z = 0
self.goal.yaw = 0 #0*np.arctan2(self.T[1],self.T[0])
self.dyaw = 0
rospy.loginfo("Ready")
else:
rospy.loginfo("Haven't received coefficients.")
elif data.command == data.CMD_INIT and self.status == FLYING:
if self.received_coeff:
rospy.loginfo("Starting")
self.go = True
else:
rospy.loginfo("Haven't received coefficients.")
if self.status == TAKEOFF:
self.goal.pos.z = utils.saturate(self.goal.pos.z+0.0025, self.alt,-0.1)
if self.status == LANDING:
if self.pose.position.z > 0.4:
# fast landing
self.goal.pos.z = utils.saturate(self.goal.pos.z - 0.0035, 2.0, -0.1)
else:
# slow landing
self.goal.pos.z = utils.saturate(self.goal.pos.z - 0.001, 2.0, -0.1)
if self.goal.pos.z == -0.1:
self.status = NOT_FLYING
self.wpType = DISABLE
rospy.loginfo("Landed!")
def test_node_exist(self):
nodes = rosnode.get_node_names()
self.assertIn('/referee_receiver', nodes, 'node does not exist')
import rospy
import math
import time
import rosnode
# Import message file
from std_msgs.msg import String
from khepera_communicator.msg import K4_controls, SensorReadings
from geometry_msgs.msg import TransformStamped
start = time.time()
rospy.init_node('Central_Algorithm', anonymous=True)
# Get all the node names for all the currently running K4_Send_Cmd nodes (all running Kheperas)
# Get the node names of all the current running nodes
node_list = rosnode.get_node_names()
# Find the nodes that contains the "K4_Send_Cmd_" title
khep_node_list = [s for s in node_list if "K4_Send_Cmd_" in s]
ip_num_list = [x[13:16] for x in khep_node_list]
khep_node_cnt = len(khep_node_list)
# Establish all the publishers to each "K4_controls_" topic, corresponding to each K4_Send_Cmd node, which corresponds to each Khepera robot
pub = []
for i in range(khep_node_cnt):
pub.append(
rospy.Publisher('K4_controls_' + str(ip_num_list[i]),
K4_controls,
queue_size=10))
import rospy
import rosnode
import math
from tf.transformations import euler_from_quaternion
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PointStamped, PoseStamped, Quaternion
#check if slam get started
ros_nodes = rosnode.get_node_names()
if ('/gmapping' not in ros_nodes) or ('/move_base' not in ros_nodes):
print('SLAM not started')
else:
print('Move in SLAM mode')
rospy.init_node('xbot_move_slam')
odom_data = rospy.wait_for_message('/odom', Odometry, timeout=5)
# TODO:
# get x_pos, y_pos, and quaternion of new goal
# x_pos =
# y_pos =
# yaw =
#create goal
goal = PoseStamped()
goal.header = odom_data.header
#cal goal_x, goal_y
goal.pose.position.x = x_pos
goal.pose.position.y = y_pos
goal.pose.position.z = 0.0
#goal.pose.orientation = euler_to_quaternion()
def _list(self):
"""
Override Namespace._list()
"""
return rosnode.get_node_names(namespace=self._ns)
def main():
# rospy.init_node("pose_groupstate_example")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(1.0)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
print("vertical")
arm.set_named_target("vertical")
arm.go()
print("vertical1")
arm.set_named_target("vertical1")
arm.go()
print("vertical")
arm.set_named_target("vertical")
arm.go()
print("vertical1")
arm.set_named_target("vertical1")
arm.go()
print("vertical")
arm.set_named_target("vertical")
arm.go()
print("home")
arm.set_named_target("home")
arm.go()
print("vertical")
arm.set_named_target("vertical")
arm.go()
print("vertical1")
arm.set_named_target("vertical1")
arm.go()
print("vertical")
arm.set_named_target("vertical")
arm.go()
print("vertical1")
arm.set_named_target("vertical1")
arm.go()
print("vertical")
arm.set_named_target("vertical")
arm.go()
print("home")
arm.set_named_target("home")
arm.go()
arm_goal_pose = arm.get_current_pose().pose
print("Arm goal pose:")
print(arm_goal_pose)
print("done")
import sys
import rospy
import subprocess
import rosnode
import numpy as np
import cv2
import time
import os
import rospkg
timestr = time.strftime("%d-%m-%Y_%H-%M-%S.png")
# Ros Messages
from sensor_msgs.msg import CompressedImage
ros_nodes = rosnode.get_node_names()
if '/raspicam_node' in ros_nodes:
command='rosservice call /camera/start_capture'
process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE)
else:
command='/home/erle/ros_catkin_ws/install_isolated/camera.sh'
command+=';rosservice call /camera/start_capture'
process = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE)
msg_image = rospy.wait_for_message('/camera/image/compressed', CompressedImage, timeout=7)
np_arr = np.fromstring(msg_image.data, np.uint8)
image_np = cv2.imdecode(np_arr, 1) #cv2.CV_LOAD_IMAGE_COLOR
rospack = rospkg.RosPack()
images_path = rospack.get_path('robot_blockly') + '/frontend/pages/images/'
cv2.imwrite(images_path+ 'image_' + timestr, image_np)
def update_nodepkgmap(self):
master = roslib.scriptutil.get_master()
nodes = rosnode.get_node_names()
for n in nodes:
self.nodepkgmap[n] = self.getnodepkg1(n)[0]
def remove_dead_nodes(self):
running_nodes = rosnode.get_node_names()
dead_nodes = [node_name for node_name in self.nodes if node_name not in running_nodes]
for node_name in dead_nodes:
self.nodes.pop(node_name, None)
#!/usr/bin/env python
from reflex_gripper.reflex_sf_hand import ReflexSFHand
from std_srvs.srv import Empty
import argparse, sys, rosnode, rospy
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Calibrate a Gripper Hand with the interactive script")
parser.add_argument('name', help="The name of the gripper hand to calibrate")
args = parser.parse_args(sys.argv[1:])
name = args.name
if '/reflex_%s'%name in rosnode.get_node_names():
print "If you do not see the script running in this window, "
print "please check the window that the hand service is running in."
rospy.wait_for_service('reflex_%s/zero_fingersI'%name, 5)
service = rospy.ServiceProxy('/reflex_%s/zero_fingersI'%name, Empty)
service()
else:
hand = ReflexSFHand(name)
hand.calibrateI()
for i in range(0,len(names)):
full_command += ['--tab', '-e',
"bash -c \"roslaunch reflex_gripper reflex_gripper.launch " +\
"Name:=%s Port:=%s\"" %(names[i],ports[i])]
subprocess.call(['gnome-terminal'] + full_command)
print "--------------------------------------------"
# Make sure the required nodes have successfully launched
print "Checking for successful launch of all required nodes..."
time.sleep(4)
found = False
for name in names:
# check that finger was successfully started
print "\tChecking %s..." % name
found = "/dynamixel_manager_%s" % name in rosnode.get_node_names()
if not bkgOnly:
found = found and "/reflex_%s" % name in rosnode.get_node_names()
if(not found):
print "%s: error: It appears that the required " %(progName) +\
"nodes were not successfully started. Please try again."
return -1
print "--------------------------------------------"
# if RRService specified try starting the RR Service
if RRService:
try:
RRpkg = rospack.get_path('reflex_gripper_rr_bridge')
print "Starting RR Services for each hand..."
print "(Each process will open a new window)"
full_command = []
def test_node_exist(self):
nodes = rosnode.get_node_names()
self.assertIn('/buzzer', nodes, "node dose not exist")
def test_node_exist(self):
nodes = rosnode.get_node_names()
self.assertIn('/wall_stop',nodes, "node does not exist")
def main (self):
##rospy.init_node("crane_x7_pick_and_place_controller")
self.run = True
self.robot = moveit_commander.RobotCommander()
self.arm = moveit_commander.MoveGroupCommander("arm")
self.gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(self.robot.get_group_names())
print("Current state:")
print(self.robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = self.arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
"""
現在縦持ちで仮定しているためコメントアウト
# 横持ち
# タンバリンをたたくために位置移動
def preparation_horizontal():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = tambourine_x_position_horizontal
target_pose.position.y = tambourine_y_position_horizontal
target_pose.position.z = tambourine_z_position_horizontal
q = quaternion_from_euler(-3.14, 0, 3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# 角度を変化させタンバリンを叩く
def hit_tambourine_horizontal():
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = tambourine_x_position_horizontal
target_pose.position.y = tambourine_y_position_horizontal
target_pose.position.z = tambourine_z_position_horizontal
q = quaternion_from_euler(stick_angle_horizontal, 0, 3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
"""
# SRDFに定義されている"home"の姿勢にする
self.arm.set_named_target("home")
self.arm.go()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical1
target_pose.position.y = self.tambourine_y_position_vertical1
target_pose.position.z = self.tambourine_z_position_vertical1
q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
self.preparation_vertical()
self.hit_tambourine_vertical()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = self.tambourine_x_position_vertical1
target_pose.position.y = self.tambourine_y_position_vertical1
target_pose.position.z = self.tambourine_z_position_vertical1
q = quaternion_from_euler(3.14 * 9 / 10, 3.14 / 2, -3.14) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
self.arm.set_pose_target(target_pose) # 目標ポーズ設定
self.arm.go() # 実行
# SRDFに定義されている"home"の姿勢にする
self.arm.set_named_target("home")
self.arm.go()
self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value)
self.arm.set_named_target("home")
self.arm.go()
"""
現在縦持ちで仮定しているためコメントアウト
#パターン-----横持ち
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.preparation_horizontal()
self.hit_tambourine_horizontal()
self.arm.set_max_velocity_scaling_factor(self.move_max_velocity_value)
self.arm.set_named_target("home")
self.arm.go()
"""
self.run = False
def update_model(self):
""" query the ros master for information about the state of the system """
# Query master and compile a list of all published topics and their types
allCurrentTopics = self._master.getPublishedTopics('/')
allCurrentTopicNames = [x[0] for x in allCurrentTopics]
# Get all the topics we currently know about
rsgTopics = self._topology.topics
# Remove any topics from Ros System Graph not currently known to master
for topic in rsgTopics.values():
if topic.name not in allCurrentTopicNames:
print "Removing Topic",topic.name, "not found in ",allCurrentTopicNames
topic.release()
# Add any topics not currently in the Ros System Graph
for topicName, topicType in allCurrentTopics:
if topicName not in rsgTopics: # and topicName not in QUIET_NAMES:
topic = Topic(self._topology,topicName,topicType)
# Compile a list of node names
allCurrentNodes = rosnode.get_node_names()
# Get all nodes we currently know about
rsgNodes = self._topology.nodes
# Remove any nodes from RosSystemGraph not currently known to master
for node in rsgNodes.values():
if node.name not in allCurrentNodes:
print "Removing Node",node.name, "not found in ",allCurrentNodes
node.release()
# Add any nodes not currently in the Ros System Graph
for name in allCurrentNodes:
if name not in rsgNodes: # and name not in QUIET_NAMES:
node = Node(self._topology,name)
try:
node.location = self._master.lookupNode(name)
except socket.error:
raise Exception("Unable to communicate with master!")
# Check for added or removed connections
systemState = self._master.getSystemState()
# Process publishers
for topicName, publishersList in systemState[0]:
if topicName in QUIET_NAMES:
continue
rsgPublishers = self._topology.topics[topicName].publishers
# Remove publishers that don't exist anymore
for publisher in rsgPublishers:
if publisher.node.name not in publishersList:
publisher.release()
# Add publishers taht are not yet in the RosSystemGraph
for nodeName in publishersList:
if nodeName not in [pub.node.name for pub in rsgPublishers]:
publisher = Publisher(self._topology,self._topology.nodes[nodeName],self._topology.topics[topicName])
# Process subscribers
for topicName, subscribersList in systemState[1]:
if topicName in QUIET_NAMES:
continue
try:
rsgSubscribers = self._topology.topics[topicName].subscribers
except:
print topicName,"not found in"
continue
# Remove subscribers that don't exist anymore
for subscriber in rsgSubscribers:
if subscriber.node.name not in subscribersList:
subscriber.release()
# Add subscriber taht are not yet in the RosSystemGraph
for nodeName in subscribersList:
if nodeName not in [sub.node.name for sub in rsgSubscribers]:
subscriber = Subscriber(self._topology,self._topology.nodes[nodeName],self._topology.topics[topicName])
self._update_view()
def test_node_exist(self):
nodes = rosnode.get_node_names()
self.assertIn('/lightsensors',nodes, "node does not exist")
def test_deadlock(self):
# Prepare condition (for safe preemption)
self.condition = threading.Condition()
self.last_execution_time = None
# Prepare Simple Action Server
self.action_server = actionlib.SimpleActionServer(
Constants.topic,
TestAction,
execute_cb=self.execute_callback,
auto_start=False)
self.action_server.register_preempt_callback(self.preempt_callback)
self.action_server.start()
# Sleep for the amount specified
rospy.sleep(Constants.deadlock_timeout)
# Start actual tests
running_nodes = set(rosnode.get_node_names())
required_nodes = {
"/deadlock_companion_1",
"/deadlock_companion_2",
"/deadlock_companion_3",
"/deadlock_companion_4",
"/deadlock_companion_5"}
self.assertTrue(required_nodes.issubset(running_nodes),
"Required companion nodes are not currently running")
# Shutdown companions so that we can exit nicely
termination_time = rospy.Time.now()
rosnode.kill_nodes(required_nodes)
rospy.sleep(Constants.shutdown_timeout)
# Check last execution wasn't too long ago...
self.assertIsNotNone(self.last_execution_time is None,
"Execute Callback was never executed")
time_since_last_execution = (
termination_time - self.last_execution_time).to_sec()
self.assertTrue(
time_since_last_execution < 2 * Constants.max_action_duration,
"Too long since last goal was executed; likely due to a deadlock")
