def update_subscribers(self):
# check connections
# for name in self.subscribers.keys():
# sub = self.subscribers[name]
# if sub.get_num_connections() == 0:
# sub.unregister()
# self.subscribers.pop(name)
# rospy.loginfo('unsubscribe (%s)',name)
# check new publishers
current_subscribers = rospy.client.get_published_topics()
for topic_info in current_subscribers:
name = topic_info[0]
typ_tuple = tuple(topic_info[1].split('/'))
typ = '%s.msg.%s'%typ_tuple
if typ in self.useless_types:
continue
if name in self.subscribers.keys():
continue
# Import and Check
try:
type_obj = eval(typ)()
except (AttributeError, NameError), e:
try:
rospy.loginfo("try to load [%s]", typ_tuple[0]);
roslib.load_manifest(typ_tuple[0])
exec('import ' + typ_tuple[0] + '.msg')
except SyntaxError, e:
rospy.loginfo('please rosmake %s', typ_tuple[0])
def readAll(self):
# hbase> get "session_table", "row_key", "column_name"
result = self.hbaseclient.get("session_table", "0", "timestamp:" + self.session_id)
num_entries = result[0].value
for i in range(1, int(num_entries) + 1):
row_time = self.hbaseclient.get("session_table", "%d" % i, "timestamp:" + self.session_id)
timestamp = row_time[0].value
print "timestamp: " + timestamp
row_topic = self.hbaseclient.get("session_table", "%d" % i, "topic:" + self.session_id)
topic = row_topic[0].value
print "topic: " + topic
row_msg = self.hbaseclient.get("session_table", "%d" % i, "msg:" + self.session_id)
msg_value = row_msg[0].value
msg_type = self.topictype_dict[topic]
print "msg type: " + msg_type
if msg_type not in msgs:
m = msg_type.split(".")
roslib.load_manifest(m[0])
_tmp = __import__(m[0] + ".msg", globals(), locals(), [m[-1]])
msgs[msg_type] = getattr(_tmp, m[-1])
obj = msgs.get(msg_type)()
obj.deserialize(msg_value)
print "msg:\n", obj, "\n"
def setup_topic_data(self):
for topic, topic_type in self.config.items():
if topic not in self.msg_parameters:
self.topic_name_list.append(topic)
# Only one type per topic
type_key = topic_type.keys()[0]
self.subscribed_list.append(type_key)
# One set of data items per topic type
self.data_items = [data_item for data_item in topic_type[type_key].keys()]
# Add data items to the MTConnect Adapter - must be unique data items
bridge_library.add_event((self.adapter, self.data_items, self.di_dict, False))
# Extract package name and topic type name
tokens = topic_type.keys()[0].split('/')
package = tokens[0]
type_name = tokens[1]
# Load package manifest if unique
if package not in self.lib_manifests:
roslib.load_manifest(package)
self.lib_manifests.append(package)
# Import module and create topic type class,
# i.e. append <class 'mtconnect_msgs.msg._RobotStates.RobotStates'>
rospy.loginfo('Class Instance --> ' + package + '.msg.' + type_name)
type_handle = getattr(import_module(package + '.msg'), type_name)
self.topic_type_list[topic] = type_handle
self.msg_text[topic] = type_handle._full_text
self.member_types[topic] = type_handle._slot_types
self.member_names[topic] = type_handle.__slots__
return
def coverage_html():
import os.path
if not os.path.isfile('.coverage-modules'):
sys.stderr.write("No .coverage-modules file; nothing to do\n")
return
with open('.coverage-modules','r') as f:
modules = [x for x in f.read().split('\n') if x.strip()]
cov = coverage.coverage()
cov.load()
# import everything
for m in modules:
try:
base = m.split('.')[0]
roslib.load_manifest(base)
__import__(m)
except:
sys.stderr.write("WARN: cannot import %s\n"%(base))
modlist = '\n'.join([" * %s"%m for m in modules])
sys.stdout.write("Generating for\n%s\n"%(modlist))
# load the module instances to pass to coverage so it can generate annotation html reports
mods = []
# TODO: rewrite, buggy
for m in modules:
mods.extend([v for v in sys.modules.values() if v and v.__name__.startswith(m) and not v in mods])
# dump the output to covhtml directory
cov.html_report(mods, directory="covhtml")
def setup_topic_data(self):
for package, action in self.config.items():
if package not in self.msg_parameters: # Only one ROS package in config by design
# Load package manifest if unique
if package not in self.lib_manifests:
roslib.load_manifest(package)
self.lib_manifests.append(package)
# Import module
rospy.loginfo('Importing --> ' + package + '.msg')
self.type_handle = import_module(package + '.msg')
# Iterate through requests and create action result class instances and action servers
for request in action:
# Capture action name for action client callback reference
self.action_list[request] = request
# Store the ROS server name in a hash table
di_conv = bridge_library.split_event(request)
self.server_name[di_conv] = request
# Create a dictionary of result class instances
request_class = getattr(self.type_handle, request + 'Result')
self._resultDict[request] = request_class()
# Create instance of the action class
action_class = getattr(self.type_handle, request + 'Action')
# Create action server for requested action, store in a hash table
self._as[request] = actionlib.SimpleActionServer(request + 'Client', action_class, self.execute_cb, False)
# Start the action server
self._as[request].start()
return
def kinfu(self, basename, type = 'list_ext'):
""" Use Kinect fusion on list of depth images, and store cloud
Usage: kinfu(basename, type = 'list_ext')
Input:
basename - Path to file that contains depth image names.
type{'list_ext'} - List file type ('list' or 'list_ext')
Output:
cloud, poses - Filenames to output cloud and poses from kinfu
"""
roslib.load_manifest('kinfu')
import kinfu
depth_textfile = self.getName(basename, type)
# Need to read list and export using full names
fullnames = list()
with open(depth_textfile, 'rt') as file_:
for name in file_:
fullnames.append( self.getName(name.rstrip(), 'depth') )
output_prefix = os.path.join(self.getPath(type), basename) + '_'
KF = kinfu.KinFuPy('file')
cloud, poses = KF.kinfu_list(fullnames, output_prefix, opts='singlepass')
# Store cloud and poses
utils.save(self.getName(basename, 'cloud'), {'cloud': cloud}, \
zipped=True)
utils.save(self.getName(basename, 'poses'), {'poses': poses}, \
zipped=True)
return cloud, poses
def get_tf_listener():
import roslib
roslib.load_manifest("tf")
import tf
return tf.TransformListener()
def __init__(self, action_name, action_type, queue_size=1):
self.name = action_name
action_type_module, action_type_name = tuple(action_type.split('/'))
roslib.load_manifest(action_type_module)
msg_module = import_module(action_type_module + '.msg')
self.rostype_name = action_type
self.rostype_action = getattr(msg_module, action_type_name + 'Action')
self.rostype_action_goal = getattr(msg_module, action_type_name + 'ActionGoal')
self.rostype_action_result = getattr(msg_module, action_type_name + 'ActionResult')
self.rostype_action_feedback = getattr(msg_module, action_type_name + 'ActionFeedback')
self.rostype_goal = getattr(msg_module, action_type_name + 'Goal')
self.rostype_result = getattr(msg_module, action_type_name + 'Result')
self.rostype_feedback = getattr(msg_module, action_type_name + 'Feedback')
self.status_msg = deque([], queue_size)
self.status_sub = rospy.Subscriber(self.name + '/' +self.STATUS_SUFFIX, actionlib_msgs.msg.GoalStatusArray, self.status_callback)
self.result_msg = deque([], queue_size)
self.result_sub = rospy.Subscriber(self.name + '/' + self.RESULT_SUFFIX, self.rostype_action_result, self.result_callback)
self.feedback_msg = deque([], queue_size)
self.feedback_sub = rospy.Subscriber(self.name + '/' +self.FEEDBACK_SUFFIX, self.rostype_action_feedback, self.feedback_callback)
self.goal_pub = rospy.Publisher(self.name + '/' + self.GOAL_SUFFIX, self.rostype_action_goal)
self.cancel_pub = rospy.Publisher(self.name + '/' +self.CANCEL_SUFFIX, actionlib_msgs.msg.GoalID)
def _loadClass(modname, subname, classname):
""" Loads the manifest and imports the module that contains the specified type
Returns the loaded module, or None on failure """
global loaded_manifests, loaded_modules
if not (modname in loaded_manifests):
try:
roslib.load_manifest(modname)
loaded_manifests[modname] = True;
except:
print "Unable to load manifest for module %s" % (modname,)
return None
fullname = modname + '.'+ subname + '._' + classname
if not (fullname in loaded_modules):
try:
loaded_modules[fullname] = __import__(fullname)
except:
print "Unable to import %s" % (fullname,)
return None
# Module has been successfully imported
return loaded_modules[fullname]
def hdf2bag(pargs):
import roslib #@UnresolvedImport
roslib.load_manifest('bootstrapping_adapter') #@UnresolvedImport
parser = OptionParser(usage=usage)
parser.disable_interspersed_args()
(options, args) = parser.parse_args(pargs) #@UnusedVariable
if len(args) != 1:
msg = 'I expect only one filename.'
raise Exception(msg)
basedir = args[0]
global_config_file = os.path.join(basedir, 'robot_info.yaml')
if not os.path.exists(global_config_file):
raise Exception('Configuration file %r not found.' %
global_config_file)
global_config = yaml.load(open(global_config_file))
print global_config.__repr__()
for hdf_filename in glob(os.path.join(basedir, '*.h5')):
bag_filename = os.path.splitext(hdf_filename)[0] + '.bag'
id_episode = os.path.basename(os.path.splitext(hdf_filename)[0])
pg('hdf2bag_conversion',
dict(hdf=hdf_filename,
bag=bag_filename,
id_robot=global_config['id_robot'],
id_actuators=global_config['id_actuators'],
id_sensors=global_config['id_sensors'],
id_episode=id_episode,
id_environment=global_config['id_environment'],
commands_spec=global_config['commands_spec']))
def testPlanePointcloud():
import roslib; roslib.load_manifest('laser_camera_segmentation')
import laser_camera_segmentation.processor as processor
import laser_camera_segmentation.configuration as configuration
cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
#sc = scanner.scanner(cfg)
pc = processor.processor(cfg)
#pc.load_data('2009Oct30_162400')
pc.load_data('2009Nov04_141226')
pc.process_raw_data()
debug = False
model = PlaneLeastSquaresModel(debug)
data = numpy.asarray(pc.pts3d_bound).T
# run RANSAC algorithm
ransac_fit, ransac_data = ransac(data,model,
3, 1000, 0.02, 300, # misc. parameters
debug=debug,return_all=True)
print ransac_fit
print ransac_data
print 'len inlier',len(ransac_data['inliers']),'shape pts',np.shape(pc.pts3d_bound)
pc.pts3d_bound = pc.pts3d_bound[:,ransac_data['inliers']]
pc.display_3d('height')
def imu_test():
g = 9.81
from sensor_msgs.msg import Imu
import roslib; roslib.load_manifest('ardrone_control')
import rospy; global rospy
import tf; global tf;
# sensor = IMU()
msg = Imu()
msg.angular_velocity.z = 1.0
msg.linear_acceleration.z = -9.81
msg.linear_acceleration.x = -0.01
msg.orientation.z = 1/sqrt(2)
msg.orientation.w = 1/sqrt(2)
# msg.linear_acceleration.z = g
# sensor.measure(msg)
# for sensor in sensor.sensors.values():
# print sensor.properties
sensor = IMU()
#sensor = IMU_Mahoney( Ts = 0.01)
#sensor = IMU_Magdwick( Ts = 0.01)
sensor.measure(msg)
print sensor.sensors['accelerometer']
print sensor.sensors['gyroscope']
print sensor.sensors['dummy_yaw']
def reload_msg_definition(self):
# generate idl module
program = ' '.join(sys.argv)
base = """// This IDL is generated by '%s'
#include "BasicDataType.idl"
module RTMROSDataBridge
{%s};
""" % (program, self.idltext)
open(self.idldir+'/'+self.idlfile, 'w').write(base)
genidl_bin = Popen(['rosrun','openrtm_aist','rtm-config','--idlc'], stdout=PIPE, stderr=PIPE).communicate()[0]
rtmpkg = Popen(['rosrun','openrtm_aist','rtm-config','--prefix'], stdout=PIPE, stderr=PIPE).communicate()[0]
genidl_bin = genidl_bin.rstrip('\n')
rtmpkg = rtmpkg.rstrip('\n')
call([genidl_bin, '-bpython', '-I%s/include/openrtm-1.1/rtm/idl'%rtmpkg, '-C'+self.idldir, self.idldir+'/'+self.idlfile])
time.sleep(1) # ??
# reload RTC data object
global RTMROSDataBridge
import RTMROSDataBridge
reload(RTMROSDataBridge)
reload(RTMROSDataBridge.rosbridge_idl)
# register ros/rtm data object type
for msg in self.generated:
rtm_typ = 'RTMROSDataBridge.' + (msg.replace('/','_'))
ros_typ_tuple = tuple(msg.split('/'))
ros_typ = '%s.msg.%s' % ros_typ_tuple
roslib.load_manifest(ros_typ_tuple[0])
exec('import ' + ros_typ_tuple[0] + '.msg')
self.msg2obj[msg] = (eval(ros_typ), eval(rtm_typ))
rospy.loginfo('Data[%s] in RTC = "%s"', msg, self.get_rtmobj(msg))
return True
def update_status(self, msg):
"""Update the known userdata and active state set and return True if the graph needs to be redrawn."""
# Initialize the return value
needs_update = False
# Check if the initial states or active states have changed
if set(msg.initial_states) != set(self._initial_states):
self._structure_changed = True
needs_update = True
if set(msg.active_states) != set(self._active_states):
needs_update = True
# Store the initial and active states
self._initial_states = msg.initial_states
self._last_active_states = self._active_states
self._active_states = msg.active_states
# Unpack the user data
while not rospy.is_shutdown():
try:
self._local_data._data = pickle.loads(msg.local_data)
break
except ImportError as ie:
# This will only happen once for each package
modulename = ie.args[0][16:]
packagename = modulename[0 : modulename.find(".")]
roslib.load_manifest(packagename)
self._local_data._data = pickle.loads(msg.local_data)
# Store the info string
self._info = msg.info
return needs_update
def create_service_proxy(url, name, service_type, binary=None):
try:
service_type_module, service_type_name = tuple(service_type.split('/'))
roslib.load_manifest(service_type_module)
srv_module = import_module(service_type_module + '.srv')
return IndividualServiceProxy(url, name, srv_module, service_type_name, binary=binary)
except Exception, e:
print "Unknown service type %s" % service_type
return None
def load_message_class(pkg, msg):
"""
Given a ROS package and message name, returns the corresponding Python
class object.
"""
roslib.load_manifest(pkg)
m = __import__(pkg+'.msg')
mod = getattr(m, 'msg')
return getattr(mod, msg)
def startup_ros(self, spin=False):
'''ROS-specific: Sets up callbacks for
- recognized speech from pocketsphinx
- world state updates
- robot state updates
and publishers for
- HandsFreeCommand
Returns:
bool: Whether the setup succeeded. Note that this won't
return until ros has shutdown if spin (== True)!
'''
# Some settings
Debug.printing = False
# Setup default system.
self.set_world()
# Setup ROS.
try:
import roslib
roslib.load_manifest('pr2_pbd_interaction')
import rospy
from std_msgs.msg import String
from pr2_pbd_interaction.msg import (
HandsFreeCommand, WorldObjects, RobotState, Description,
HandsFreeGrounding)
from pr2_pbd_interaction.srv import WorldChange
# TODO(mbforbes); This waits for ROS. This is annoying, but
# actually may be OK for now.
rospy.init_node('hfpbd_parser', anonymous=True)
# We get: speech, world objects, robot state.
rospy.Subscriber('recognizer/output', String, self.sphinx_cb)
rospy.Subscriber(
'handsfree_robotstate', RobotState, self.robot_state_cb)
# We send: parsed commands, grounding results.
self.hfcmd_pub = rospy.Publisher(
'handsfree_command', HandsFreeCommand)
self.hfgrounding_pub = rospy.Publisher(
'handsfree_grounding', HandsFreeGrounding)
# We provide: descriptions for objects.
rospy.Service(
'handsfree_worldchange', WorldChange, self.handle_world_change)
# Setup complete
self.ros_running = True
# If no other frontend, just wait here.
if spin:
rospy.spin()
return True
except ImportError:
# We don't have ROS installed! That's OK.
return False
def __getattr__(self, name):
try:
return getattr(self.wrapped, name)
except AttributeError:
import roslib.packages
try:
roslib.load_manifest(name.split('.')[0])
except roslib.packages.InvalidROSPkgException as e:
raise ImportError("Cannot import module '%s': \n%s"%(name, str(e)))
return __import__(name)
def test_arm_fk():
roslib.load_manifest('tf'); import tf as rostf
listener = rostf.TransformListener()
rospy.sleep(.5)
now = rospy.Time.now()
listener.waitForTransform("base_link", "r_gripper_tool_frame", now,rospy.Duration(1))
xyz, quat = listener.lookupTransform("base_link", "r_gripper_tool_frame",now)
posemat = brett.larm.get_pose_matrix("base_link", "r_gripper_tool_frame")
assert np.allclose(xyz, posemat[:3,3],atol = 1e-4)
def create_topic_proxy(url, name, topic_type, pub=True, sub=True, publish_interval=None, binary=None):
try:
topic_type_module, topic_type_name = tuple(topic_type.split('/'))
roslib.load_manifest(topic_type_module)
msg_module = import_module(topic_type_module + '.msg')
return IndividualTopicProxy(url, name, msg_module, topic_type_name,
pub=pub, sub=sub, publish_interval=publish_interval, binary=binary)
except Exception, e:
print "Unknown msg type %s" % topic_type
return None
def _get_message_or_service_class(type_str, message_type, reload_on_error=False):
"""
Utility for retrieving message/service class instances. Used by
get_message_class and get_service_class.
:param type_str: 'msg' or 'srv', ``str``
:param message_type: type name of message/service, ``str``
:returns: Message/Service for message/service type or None, ``class``
:raises: :exc:`ValueError` If message_type is invalidly specified
"""
if message_type == 'time':
return Time
if message_type == 'duration':
return Duration
## parse package and local type name for import
package, base_type = genmsg.package_resource_name(message_type)
if not package:
if base_type == 'Header':
package = 'std_msgs'
else:
raise ValueError("message type is missing package name: %s"%str(message_type))
pypkg = val = None
try:
# import the package
pypkg = __import__('%s.%s' % (package, type_str))
except ImportError:
# try importing from dry package if available
try:
from roslib import load_manifest
from rospkg import ResourceNotFound
try:
load_manifest(package)
try:
pypkg = __import__('%s.%s' % (package, type_str))
except ImportError:
pass
except ResourceNotFound:
pass
except ImportError:
pass
if pypkg:
try:
val = getattr(getattr(pypkg, type_str), base_type)
except AttributeError:
pass
# this logic is mainly to support rosh, so that a user doesn't
# have to exit a shell just because a message wasn't built yet
if val is None and reload_on_error:
try:
if pypkg:
reload(pypkg)
val = getattr(getattr(pypkg, type_str), base_type)
except:
val = None
return val
def init_ros(m):
print "ROS init start"
print m
import roslib; roslib.load_manifest('django_crowd_server')
import rospy
rospy.init_node("django_crowd_server",disable_signals=True,anonymous=True)
from annotation_publisher import PublishAnnotationNode
m["sender"]=PublishAnnotationNode("annotation_publisher");
print "ROS init done"
def nothing():
import rospy
rospy.init_node('benk',anonymous=True,disable_signals=True)
import roslib
roslib.load_manifest('tf')
import tf.transformations as tft
import numpy
from brett2.PR2 import PR2
q = numpy.array([0.0037790022003,0.0621387511329,0.624889472514,0.778228364249])
p = numpy.array([0.516622185707,0.0262447148561,0.845655560493])
def __init__(self, btn_index, callback=(lambda : 0)):
self.btn_index = btn_index
self.callback = callback
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('joy')
from joy.msg import Joy
import rospy
self.rospy = rospy
rospy.init_node('joy_trigger', disable_signals=True)
self.sub = rospy.Subscriber('/joy', Joy, self._joy_callback)
self.btn_state = -1
def initialize_ros(self):
for inp in self.inputs.values():
type_split = inp.type.split('/')
if len(type_split) != 2:
raise Exception('invalid type')
module = type_split[0]
roslib.load_manifest(module)
module += '.msg'
message = type_split[1]
mod = __import__(module, globals(), locals(), [message])
print("Subscribing to %s (%s)"%(inp.name, inp.type))
self.subscribers.append(rospy.Subscriber(inp.name, getattr(mod, message), self.handle_ros_message))
def rosload_func(self, pkg_name):
"""
Import roslib, load the manifest for the given package, and import rospy.
"""
import roslib
roslib.load_manifest(pkg_name)
import rospy
try:
__import__(pkg_name)
except ImportError:
print "Package does not appear to have a python module"
def run_test():
try:
if len(sys.argv) > 1 and sys.argv[1].startswith("--gtest_output="):
import roslib; roslib.load_manifest('multi_interface_roam')
global rostest
import rostest
run_ros_tests()
else:
import unittest
unittest.main()
exitval.append(0)
except SystemExit, v:
exitval.append(v.code)
def __init__(self, service_name, service_type):
self.name = service_name
service_type_module, service_type_name = tuple(service_type.split('/'))
roslib.load_manifest(service_type_module)
srv_module = import_module(service_type_module + '.srv')
self.rostype_name = service_type
self.rostype = getattr(srv_module, service_type_name)
self.rostype_req = getattr(srv_module, service_type_name + 'Request')
self.rostype_resp = getattr(srv_module, service_type_name + 'Response')
self.proxy = rospy.ServiceProxy(self.name, self.rostype)
def add_moos2ros_map(self, name, params):
# Make sure the msg_type is valid
split_msg_type = params['msg_type'].split('/')
if len(split_msg_type) != 2:
raise ValueError("Invalid msg_type given by " +
"%s, should take the form ros_pkg/MsgType" % name)
msg_pkg, msg_type = split_msg_type
try:
roslib.load_manifest(msg_pkg)
eval("from %s.msgs import %s as msg_obj" % (msg_pkg, msg_type))
except (ImportError, InvalidROSPkgException) as e:
raise ValueError("Cannot find msg_type given by " +
"%s, should take the form ros_pkg/MsgType: %s" % (name, str(e)))
def load_pkg_module(package, directory):
#check if its in the python path
path = sys.path
try:
imp.find_module(package)
except:
roslib.load_manifest(package)
try:
m = __import__( package + '.' + directory )
except:
rospy.logerr( "Cannot import package : %s"% package )
rospy.logerr( "sys.path was " + str(path) )
return None
return m
#!/usr/bin/env python
import roslib
roslib.load_manifest('mrobot_teleop')
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Control Your mrobot!
---------------------------
Moving around:
u i o
j k l
m , .
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
CTRL-C to quit
"""
moveBindings = {
'i': (1, 0),
'o': (1, -1),
'j': (0, 1),
'l': (0, -1),
'u': (1, 1),
',': (-1, 0),
#!/usr/bin/env python
"""Test basic functionality of the Penn Pipeline over ROS."""
import roslib
roslib.load_manifest('test_nlp')
import sys
import rospy
from nlp.srv import String
PIPELINE_SERVICE = "penn_nlp_pipeline_service"
def parse_input():
"""Request a parse for the given text."""
print "Waiting for service {}...".format(PIPELINE_SERVICE)
rospy.wait_for_service(PIPELINE_SERVICE)
try:
pipeline = rospy.ServiceProxy(PIPELINE_SERVICE, String)
while True:
user_input = raw_input("> ")
response = pipeline(user_input)
print response
except rospy.ServiceException, err:
print >> sys.stderr, "Service call failed: {}".format(err)
except (KeyboardInterrupt, EOFError):
pass
#!/usr/bin/env python
import roslib
roslib.load_manifest('tuw_gazebo')
import rospy
import actionlib
import time
import tf
from visualization_msgs.msg import *
from std_msgs.msg import String
from geometry_msgs.msg import *
from gazebo_msgs.msg import *
#from pudb import set_trace; set_trace()
robot_name = "r1"
covarianceXY = 0.1
covarianceAlpha = 0.05
class SetRobotNode():
def __init__(self):
rospy.init_node('goal_test_client')
rospy.on_shutdown(self.shutdown)
self.pub_command_topic_name = robot_name + '/cmd_vel'
self.pub_command = rospy.Publisher(self.pub_command_topic_name,
Twist,
queue_size=10)
self.pub_model_state_topic_name = 'gazebo/set_model_state'
self.pub_model_state = rospy.Publisher(self.pub_model_state_topic_name,
ModelState,
#!/usr/bin/env python
from __future__ import division
import roslib
roslib.load_manifest('experiments')
import rospy
import numpy as np
import ExperimentLib
from geometry_msgs.msg import Point, Twist
from experiment_srvs.srv import Trigger, ExperimentParamsChoices, ExperimentParamsChoicesRequest
from flycore.msg import MsgFrameState
from galvodirector.msg import MsgGalvoCommand
from ledpanels.msg import MsgPanelsCommand
from patterngen.msg import MsgPattern
#######################################################################################################
class Experiment():
def __init__(self):
rospy.init_node('Experiment')
# Fill out the data structure that defines the experiment.
self.experimentparams = ExperimentParamsChoicesRequest()
self.experimentparams.experiment.description = 'Navigation with food ysa'
self.experimentparams.experiment.maxTrials = -1
self.experimentparams.experiment.timeout = -1
# self.experimentparams.save.filenamebase = 'foodnavi_%s_%s_%s_' % (flyspec, foodspec, gender)
self.experimentparams.save.filenamebase = 'foodnavi'
self.experimentparams.save.csv = True
self.experimentparams.save.bag = True
#!/usr/bin/env python
import roslib; roslib.load_manifest('usb2dynamixel_widomx')
import rospy
from dynamixel_controllers.srv import *
from std_msgs.msg import Float64, Float64MultiArray
import numpy as np
import argparse
import time
INNER_LOOP_HZ = 15
current_torque = 0
target_torque = 0
steps = 0
timeout = 0
zero_torque = 1
set_torque_cb(data):
zero_torque = 0 #torque not zero env start torque controller
current_torque = target_torque
target_torque = data
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("joint_name", type = str, \
help = "name of this joint")
parser.add_argument("service_name", type = str,\ #joint*_controller/set_torque_limit
help = "name of the service control the torque limit")
parser.add_argumemt("command_topic_name", type = str, \ #joint*_controller/command
help = "name of the topic control the position of this servo")
#!/usr/bin/env python
from __future__ import print_function
import roslib
roslib.load_manifest('robot_guidance')
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import Float32, Int8
import numpy as np
class dummy_robot:
def __init__(self):
rospy.init_node('dummy_robot', anonymous=True)
self.bridge = CvBridge()
self.image_pub = rospy.Publisher("image_raw", Image, queue_size=1)
self.reward_pub = rospy.Publisher("reward", Float32, queue_size=1)
self.action_sub = rospy.Subscriber("action", Int8,
self.callback_action)
self.action = 0
self.pan = 0
self.size = 0
self.reward = 0
self.reward_lr = 0
self.reward_fb = 0
self.cv_image = np.zeros((480, 640, 3), np.uint8)
self.cv_image.fill(255)
self.image = self.bridge.cv2_to_imgmsg(self.cv_image, encoding="bgr8")
self.arrow_cv_image = np.zeros((200, 640, 3), np.uint8)
#! /usr/bin/env python
import roslib
roslib.load_manifest('cob_sound')
import rospy
# Brings in the SimpleActionClient
import actionlib
# Brings in the messages used by the fibonacci action, including the
# goal message and the result message.
from cob_sound.msg import *
def say_client():
# Creates the SimpleActionClient, passing the type of the action
# (FibonacciAction) to the constructor.
client = actionlib.SimpleActionClient('sound_controller/say', SayAction)
# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
goal = SayGoal()
goal.text.data = "Hello, how are you? I am fine."
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
'x0var': 0,
'noisy_body_idx': np.array([]),
'noisy_body_var': np.array([]),
'pos_body_idx': np.array([]),
'pos_body_offset': np.array([]),
'smooth_noise': True,
'smooth_noise_var': 2.0,
'smooth_noise_renormalize': True,
}
try:
import rospkg
import roslib
roslib.load_manifest('rl_msgs')
# AgentROS
AGENT_ROS = {
#TODO: It might be worth putting this in JSON/yaml format so C++
# can read it.
'trial_command_topic':
'gps_controller_trial_command',
'reset_command_topic':
'gps_controller_position_command',
'relax_command_topic':
'gps_controller_relax_command',
'data_request_topic':
'gps_controller_data_request',
'sample_result_topic':
'gps_controller_report',
import roslib; roslib.load_manifest('smach_ros')
import rospy
import threading
import traceback
import copy
from actionlib.simple_action_client import SimpleActionClient, GoalStatus
import smach
from smach.state import *
__all__ = ['SimpleActionState']
class SimpleActionState(State):
"""Simple action client state.
Use this class to represent an actionlib as a state in a state machine.
"""
# Meta-states for this action
WAITING_FOR_SERVER = 0
INACTIVE = 1
ACTIVE = 2
PREEMPTING = 3
COMPLETED = 4
def __init__(self,
# Action info
action_name,
#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #* #* Author: Eitan Marder-Eppstein #*********************************************************** PKG = 'image_cb_detector' NAME = 'image_cb_detector_node' import roslib; roslib.load_manifest(PKG) import rospy import cv import math import threading import numpy import tf from sensor_msgs.msg import Image, CameraInfo from image_cb_detector.msg import ObjectInImage from geometry_msgs.msg import PoseStamped from cv_bridge import CvBridge, CvBridgeError class ImageCbDetector:
Publishes:
-
Actions:
@author: Robotnik Automation
Software License Agreement (BSD License)
Copyright (c) 2015 Robotnik Automation SLL. All Rights Reserved.
EDITED BY GHOSTMAN @ 2018
"""
import roslib
roslib.load_manifest('phantomx_reactor_arm_controller')
import rospy
import actionlib
from control_msgs.msg import *
from std_msgs.msg import *
from sensor_msgs.msg import JointState
class WidowxGripper:
"""
Class to communicate with the dinamyxel controllers of the arm
"""
def __init__(self):
self.distance = 0.0
#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#* POSSIBILITY OF SUCH DAMAGE.
#***********************************************************
# Author: Blaise Gassend
# Reads fingertip pressure data from /pressure and publishes it as a
# visualization_marker
import roslib
roslib.load_manifest('fingertip_pressure')
import rospy
import threading
from fingertip_pressure.msg import PressureInfo, PressureInfoElement
from pr2_msgs.msg import PressureState
from visualization_msgs.msg import Marker
from fingertip_pressure.colormap import color
from geometry_msgs.msg import Vector3
positions = [ # x, y, z, xscale, yscale, zscale
(0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
(0.010, 0.010, -0.008, 0.030, 0.010, 0.010),
(0.025, 0.010, -0.008, 0.010, 0.010, 0.010),
(0.030, 0.010, -0.004, 0.010, 0.010, 0.010),
(0.030, 0.010, 0.004, 0.010, 0.010, 0.010),
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib
roslib.load_manifest('hadsafha')
import rospy
import sys
from hadsafha.terrorist_detector import TerroristDetector
from hadsafha.air_traffic_manager import AirTrafficManager
from hadsafha.zephyr_controller import ZephyrController
from hadsafha.iris_controller import IrisController
from hadsafha.task_planner import TaskPlanner
from hadsafha.comm_manager import CommManager
from hadsafha.sensor_manager import SensorManager
from hadsafha.score_manager import ScoreManager
from hadsafha.battery_manager import BatteryManager
from hadsafha.autonomous_intelligence import AutonomousIntelligence
import hadsafha.scenario as scenario
from datetime import datetime
if __name__ == "__main__":
rospy.init_node('uav_controller')
# Alınan arguman "_" karakteri ile parçalanır ve bu sayede uav tipi ve index'i seçilmiş olur
#args= sys.argv[1].split("_")
#uav_index = int(args[0])
#uav_type = args[1]
uav_index = int(sys.argv[1])
#!/usr/bin/env python
PKG = 'depth_sensor'
import roslib
roslib.load_manifest(PKG)
import serial
import math
import rospy
from kraken_msgs.msg import dvlData
#import numpy
depth = serial.Serial()
depth.name = '/dev/ttyUSB0'
depth.baudrate = 9600
depth.stopbits = 1
pub = rospy.Publisher('/kraken/depthData', depthData, queue_size=10)
rospy.init_node('depthdata', anonymous=True)
def readResponse():
response = ''
garbage = ''
while (garbage != '\r'):
garbage = depth.read(1)
response += garbage
return response
#!/usr/bin/env python
from __future__ import division
import roslib
roslib.load_manifest('actuator_source')
import rospy
import threading
import actionlib
import actions.msg
import msg_and_srv.msg
class ActuatorActionServer(object):
def __init__(self):
self.lock = threading.Lock()
self.initialized = False
self.abort = False
self.setpt_update_rate = rospy.get_param("actuator_update_rate", 50)
self.max_value = rospy.get_param("actuator_max_value", 2000)
self.min_value = rospy.get_param("actuator_min_value", 1000)
self.default_value = rospy.get_param("actuator_default_value", 1500)
self.rate = rospy.Rate(self.setpt_update_rate)
self.dt = 1 / self.setpt_update_rate
self.value_final = None
# Setup action server
self.feedback = actions.msg.ActuatorOutscanFeedback()
self.result = actions.msg.ActuatorOutscanResult()
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import roslib
roslib.load_manifest('turtle_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('tf_turtle')
listener = tf.TransformListener()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
# -*- coding: utf-8 -*-
'''
ROS node in charge of interfacing with the encoders phidget. Publishes EncoderCounts
messages on the "encoder_counts" topic.
Parameters:
- period: publish only after this period has elapsed (default value: 0.02).
This limits the publishing rate. It seems the phidget publishes a count
change every 8ms.
- min_pub_period: if set, we want to publish messages with this period, even if
encoders are not changing. Otherwise, publish only when the encoders value is
changing.
'''
import roslib; roslib.load_manifest('phidget_encoders')
import rospy
import threading
from Phidgets.PhidgetException import *
from Phidgets.Events.Events import *
from Phidgets.Devices.Encoder import Encoder
from phidget_encoders.msg import EncoderCounts
import diagnostic_updater as DIAG
def err(e):
'''A helper function to report Phidget errors'''
rospy.logerr("Phidget error %i: %s. Exiting..." % (e.code, e.details))
exit(1)
#!/usr/bin/env python
import roslib
roslib.load_manifest('collvoid_controller')
import rospy
import string
import actionlib
import math
import random
#from collvoid_local_planner.srv import InitGuess
from std_msgs.msg import String
from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped
from nav_msgs.msg import Odometry
from socket import gethostname
from collvoid_msgs.msg import PoseTwistWithCovariance
from move_base_msgs.msg import *
import tf
THRESHOLD = 0.22
def dist(a, b):
return math.sqrt(
math.pow(a.position.x - b.position.x, 2) +
math.pow(a.position.y - b.position.y, 2))
class ControllerRobots():
def __init__(self):
self.initialize()
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. # # # Author: Advait Jain ([email protected]), Healthcare Robotics Lab, Georgia Tech import roslib roslib.load_manifest('move_arm_tutorials') import rospy import actionlib import geometric_shapes_msgs from move_arm_msgs.msg import MoveArmAction from move_arm_msgs.msg import MoveArmGoal from motion_planning_msgs.msg import SimplePoseConstraint from motion_planning_msgs.msg import PositionConstraint from motion_planning_msgs.msg import OrientationConstraint from actionlib_msgs.msg import GoalStatus
#!/usr/bin/env python
import roslib
roslib.load_manifest('velma_task_cs_ros_interface')
import rospy
import math
import PyKDL
from threading import Thread
from velma_common import *
from rcprg_planner import *
from rcprg_ros_utils import MarkerPublisher, exitError
from moveit_msgs.msg import AttachedCollisionObject, CollisionObject
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import Pose
from visualization_msgs.msg import Marker
import tf_conversions.posemath as pm
class MarkerPublisherThread:
def threaded_function(self, obj):
pub = MarkerPublisher("attached_objects")
while not self.stop_thread:
pub.publishSinglePointMarker(PyKDL.Vector(),
1,
r=1,
g=0,
b=0,
a=1,
#!/usr/bin/env python
from __future__ import print_function
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy
import numpy as np
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Vector3
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
-- ROBOT TELEOP KEY --
----------------------------------
===== robot2 general command =====
q w e
a s d
x
w = increase linear speed x by 0.25
x = decrease linear speed x by 0.25
a = increase linear speed y by 0.25
d = decrease linear speed y by 0.25
q = increase angular speed z by 0.25
e = decrease angular speed z by 0.25
CTRL-C to quit
"""
#skill, position, info, goalkeeper command
movement = {
#!/usr/bin/env python
import roslib
roslib.load_manifest('RelocSensorDriver')
import tf
import rospy
import math
import time
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf.transformations import euler_from_quaternion
pioneer1 = [-0.433, -0.224, -0.000, -0.992151563931, 0.125041090001]
pioneer2 = [2.367, -1.658, -0.000, -0.992151563931, 0.125041090001]
drone1 = [4.73816204071, -0.1, 0.0, -0.992151563931, 0.125041090001]
#todo launch this along wih odometers and update odom frame to map
def talker():
rospy.init_node('RelocNode')
br = tf.TransformBroadcaster()
rospy.Subscriber('/pioneer1/initialpose',
PoseWithCovarianceStamped,
callback1,
queue_size=1)
rospy.Subscriber('/pioneer2/initialpose',
PoseWithCovarianceStamped,
callback2,
queue_size=1)
rospy.Subscriber('/drone1/initialpose',
PoseWithCovarianceStamped,
#!/usr/bin/env python3
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import turtlesim.msg
def handle_turtle_pose(msg, turtlename):
br = tf.TransformBroadcaster()
br.sendTransform((msg.x, msg.y, 0),
tf.transformations.quaternion_from_euler(0, 0, msg.theta),
rospy.Time.now(), turtlename, "world")
if __name__ == '__main__':
rospy.init_node('turtle_tf_broadcaster')
turtlename = rospy.get_param('~turtle')
rospy.Subscriber('/%s/pose' % turtlename, turtlesim.msg.Pose,
handle_turtle_pose, turtlename)
rospy.spin()
#! /usr/bin/env python
import roslib
roslib.load_manifest('cotesys_ros_grasping')
import rospy
import actionlib
from cotesys_ros_grasping.msg import AttachBoundingBoxAction, AttachBoundingBoxGoal
if __name__ == '__main__':
rospy.init_node('test_attach_bounding_box')
attach_object_client = actionlib.SimpleActionClient(
'attach_bounding_box', AttachBoundingBoxAction)
attach_object_client.wait_for_server()
goal = AttachBoundingBoxGoal()
goal.arm_name = "right_arm"
goal.x_size = .03
goal.y_size = .03
goal.z_size = .2
attach_object_client.send_goal(goal)
print 'sent goal'
attach_object_client.wait_for_result()
rospy.sleep(3)
goal.remove = True
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$
import roslib
roslib.load_manifest('test_roslaunch')
import os, sys, unittest
import time
import rostest
import roslaunch.pmon
import roslaunch.server
import roslib.xmlrpc
from xmlrpclib import ServerProxy
from roslaunch.server import ProcessListener
class MockProcessListener(ProcessListener):
#!/usr/bin/env python
import roslib
roslib.load_manifest('ros_basics')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('turtle_tf_listener')
listener = tf.TransformListener()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
turtle_vel = rospy.Publisher('turtle2/cmd_vel',
geometry_msgs.msg.Twist,
queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform('/turtle2', '/turtle1',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
#!/usr/bin/env python
""" match_template_simple.py - Version 1.0 2012-02-27
"""
import roslib
roslib.load_manifest('rbx1_vision')
import rospy
from ros2opencv2 import ROS2OpenCV2
from sensor_msgs.msg import Image, RegionOfInterest
import sys
import cv2.cv as cv
import cv2
import numpy as np
class MatchTemplate(ROS2OpenCV2):
def __init__(self, node_name):
ROS2OpenCV2.__init__(self, node_name)
self.node_name = node_name
self.use_depth_for_detection = rospy.get_param(
"~use_depth_for_detection", False)
self.fov_width = rospy.get_param("~fov_width", 1.094)
self.fov_height = rospy.get_param("~fov_height", 1.094)
self.max_object_size = rospy.get_param("~max_object_size", 0.28)
# Intialize the detection box
self.detect_box = None
#! /usr/bin/python
import roslib
roslib.load_manifest('simple_arm_controller')
import rospy
import actionlib
import time
from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, Pr2GripperCommandGoal, Pr2GripperCommandAction
from trajectory_msgs.msg import JointTrajectoryPoint
#to control the left arm:
whicharm = 'l'
rospy.init_node('controller_manager')
joint_trajectory_action_name = whicharm + '_arm_controller/joint_trajectory_action'
joint_action_client = actionlib.SimpleActionClient(
joint_trajectory_action_name, JointTrajectoryAction)
print "waiting for server..."
joint_action_client.wait_for_server()
print "got server"
joint_names = [
"_shoulder_pan_joint", "_shoulder_lift_joint", "_upper_arm_roll_joint",
"_elbow_flex_joint", "_forearm_roll_joint", "_wrist_flex_joint",
"_wrist_roll_joint"
]
joint_names1 = [whicharm + x for x in joint_names]
goal = JointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.get_rostime()
goal.trajectory.joint_names = joint_names1
import numpy as np
import copy
import roslib
roslib.load_manifest('hrl_generic_arms')
import rospy
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from geometry_msgs.msg import Transform, TransformStamped, Vector3
import tf.transformations as tf_trans
class PoseConverter:
@staticmethod
def _make_generic(args):
if type(args[0]) == str:
frame_id = args[0]
header, homo_mat, rot_quat, rot_euler = PoseConverter._make_generic(
args[1:])
if header is None:
header = [0, rospy.Time.now(), '']
header[2] = frame_id
return header, homo_mat, rot_quat, rot_euler
if len(args) == 1:
if type(args[0]) is Pose:
homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg(
args[0])
return None, homo_mat, rot_quat, rot_euler
elif type(args[0]) is PoseStamped:
homo_mat, rot_quat, rot_euler = PoseConverter._extract_pose_msg(
#!/usr/bin/python
import roslib
roslib.load_manifest('people_velocity_tracker')
import rospy
import tf
import math
from geometry_msgs.msg import Point, Vector3
from geometry_msgs.msg import Twist, TwistWithCovariance
from people_msgs.msg import PositionMeasurementArray, Person, People
from easy_markers.generator import MarkerGenerator, Marker
from kalman_filter import Kalman
def distance(leg1, leg2):
return math.sqrt(
math.pow(leg1.x - leg2.x, 2) + math.pow(leg1.y - leg2.y, 2) +
math.pow(leg1.z - leg2.z, 2))
def average(leg1, leg2):
return Point((leg1.x + leg2.x) / 2, (leg1.y + leg2.y) / 2,
(leg1.z + leg2.z) / 2)
def add(v1, v2):
return Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z)
def subtract(v1, v2):
return Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z)
