class Example(object):
def __init__(self):
# Create a D(ynamic)DynamicReconfigure
self.ddr = DDynamicReconfigure("class_example")
# Add variables (name, description, default value, min, max, edit_method)
self.ddr.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
self.ddr.add_variable("integer", "integer variable", 0, -1, 1)
self.ddr.add_variable("bool", "bool variable", True)
self.ddr.add_variable("string", "string variable", "string dynamic variable")
enum_method = self.ddr.enum([ self.ddr.const("Small", "int", 0, "A small constant"),
self.ddr.const("Medium", "int", 1, "A medium constant"),
self.ddr.const("Large", "int", 2, "A large constant"),
self.ddr.const("ExtraLarge", "int", 3, "An extra large constant")],
"An enum example")
self.ddr.add_variable("enumerate", "enumerate variable", 1, 0, 3, edit_method=enum_method)
self.add_variables_to_self()
self.ddr.start(self.dyn_rec_callback)
def add_variables_to_self(self):
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__setattr__(var_name, None)
def dyn_rec_callback(self, config, level):
rospy.loginfo("Received reconf call: " + str(config))
# Update all variables
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__dict__[var_name] = config[var_name]
return config
class ReconfigServer(object):
def __init__(self, config):
"""
Read the defaults from the config object
so that we dont change the launch-defaults right away
with different reconfig defaults
We then just put whatever reconfig we get into the BB with the same key
"""
self.bb = pt.blackboard.Blackboard()
# DynamicDynamicReConfig
# because the .cfg way of doing this is pai1
self.ddrc = DDynamicReconfigure("smarc_bt_reconfig")
# name, description, default value, min, max, edit_method
self.ddrc.add_variable(bb_enums.MIN_ALTITUDE,
"default:{}".format(config.MIN_ALTITUDE),
config.MIN_ALTITUDE, -2, 50)
self.ddrc.add_variable(bb_enums.MAX_DEPTH,
"default:{}".format(config.MAX_DEPTH),
config.MAX_DEPTH, 0, 50)
self.ddrc.add_variable(
bb_enums.MAX_ALT_FAIL_SECONDS,
"default:{}".format(config.MAX_ALT_FAIL_SECONDS),
config.MAX_ALT_FAIL_SECONDS, 0, 10)
rospy.loginfo("Started dynamic reconfig server with keys:{}".format(
self.ddrc.get_variable_names()))
# this should be the last thing in this init
self.ddrc.start(self.reconfig_cb)
def reconfig_cb(self, config, level):
for key in self.ddrc.get_variable_names():
new_value = config.get(key)
old_value = self.bb.get(key)
if old_value is None or old_value != new_value:
rospy.loginfo("New value for:{} set to:{} (was {})".format(
key, new_value, old_value))
self.bb.set(key, new_value)
return config
class ArbiterLite(object):
"""
The ArbiterLite transforms high-level commands into low-level commands by using utilities from the filters.py file.
It subscribes to various topics of the form high_level/cmd_**, then republishes as cmd_vel, takeoff, and land
The publishing of this node will run at the configured frequency, but one-time commands like takeoff and land will always
be handled immediately.
One copy of this node is intended to be created for each physical drone being controlled.
"""
def __init__(self):
self.tf = TransformListener()
self.ddynrec = DDynamicReconfigure("arbiterlite_configuration")
# Transformers are functions capable of processing incoming data in a variety of formats.
# They are functions that take input of whatever type the topic is, and produce a transformers.Command
# object.
alt_pid = transformers.PIDAltController(self.tf, self.ddynrec)
pos_pid = transformers.PIDPosController(self.tf, self.ddynrec, alt_pid)
pos_cam_pid = transformers.PIDPosCamController(self.tf, self.ddynrec, pos_pid)
# topic_name : (type, transformer_func, publish_immediately)
self.transformers = {
'high_level/cmd_vel': (Twist, transformers.cmd_vel, False),
'high_level/cmd_takeoff': (Empty, transformers.cmd_takeoff, True),
'high_level/cmd_land': (Empty, transformers.cmd_land, True),
'high_level/cmd_pos': (PoseStamped, pos_pid.cmd_pos, False),
'high_level/cmd_rel_pos': (PoseStamped, pos_pid.cmd_pos, False),
'high_level/cmd_vel_alt': (VelAlt, alt_pid.cmd_vel_alt, False),
'high_level/cmd_cam_pos': (PosCam, pos_cam_pid.cmd_pos_cam, False),
}
self.last_topic = None
self.last_msg = None
self.last_timestamp = None
for topic, (msgtype, _, immediate) in self.transformers.items():
def handler(msg, topic=topic, immediate=immediate):
self.last_topic = topic
self.last_msg = msg
self.last_timestamp = rospy.Time.now()
if immediate:
self.update()
rospy.Subscriber(topic, msgtype, handler)
# Filters constrain final output before it goes to the drone.
# Potential examples include last-minute obstacle avoidance, speed limiters, or arena boundary constraints.
self.filters = [filters.make_speed_filter(0.2, 0.2, 0.2)]
self.vel_pub = rospy.Publisher('low_level/cmd_vel', Twist, queue_size=0)
self.takeoff_pub = rospy.Publisher('low_level/takeoff', Empty, queue_size=0)
self.land_pub = rospy.Publisher('low_level/land', Empty, queue_size=0)
self.rate = rospy.get_param('~rate', 20)
# How long to republish an old message before erroring out (seconds)
self.timeout = rospy.get_param('~timeout', 1.0)
self.start_ddynrec()
def update(self):
"""
Computes any PID loops and publishes the low-level commands
"""
if self.last_topic is None:
rospy.logwarn_throttle(
5.0, "No messages recieved, unable to publish")
return
if rospy.Time.now() - self.last_timestamp > rospy.Duration.from_sec(self.timeout):
rospy.logwarn_throttle(
5.0, "Last message to {} was stale, refusing to publish".format(self.last_topic))
# TODO(eric) Consider publishing a "stop" command here, rather than relying on the timeout in the drone's driver
return
_, func, _ = self.transformers[self.last_topic]
# Convert to a transformers.Command
cmd = func(self.last_msg)
# Limit the drone speed, and apply any other filters
for func in self.filters:
cmd = func(cmd)
# Publish the result to the ROS network
if cmd.takeoff:
self.takeoff_pub.publish(Empty())
vel = Twist()
vel.linear.z = 0.5
self.vel_pub.publish(vel)
elif cmd.land:
self.land_pub.publish(Empty())
vel = Twist()
vel.linear.z = -0.5
self.vel_pub.publish(vel)
else:
self.vel_pub.publish(cmd.vel)
def start_ddynrec(self):
"""
Helper function to start the ddynamic reconfigure server with a callback
function that updates the self.ddynrec attribute.
Needed to tune PID loops in realtime
"""
def callback(config, level):
"""
A callback function used to as the parameter in the ddynrec.start() function.
This custom callback function updates the state of self.ddynrec so we can
refer to its variables whenever we have access to it.
"""
rospy.loginfo("Received reconf call: " + str(config))
# Update all variables
var_names = self.ddynrec.get_variable_names()
for var_name in var_names:
self.ddynrec.__dict__[var_name] = config[var_name]
return config
self.ddynrec.start(callback)
def run(self):
r = rospy.Rate(self.rate)
while not rospy.is_shutdown():
if self.last_topic is not None:
_, _, immediate = self.transformers[self.last_topic]
if not immediate:
# For any topic that is not supposed to be handled immediately upon reception,
# calculate the PID loop here at a defined frequency.
self.update()
try:
r.sleep()
except rospy.ROSTimeMovedBackwardsException:
r = rospy.Rate(self.rate)
class Arbiter:
"""
The Arbiter is a mutiplexer that reads cmd_* topics from several namespaces, converts them into
standard cmd_vel form, and forwards one of them into the global namespace.
It receives information about which behavior (namespace) should be selected from String messages
on the /arbiter/activate_behavior topic. This information comes from the planning stack.
In the future, a voting system could replace this mechanism.
The special value "zero" represents an internal behavior that stops the vehicle.
It also publishes the name of the active behavior to arbiter/active_behavior.
"""
def __init__(self):
# The Null behavior will aafutomatically process a 0-velocity Twist at 10hz
self.null_behavior = Behavior(self.process_command, 'zero', freq=10)
self.behaviors = {'zero': self.null_behavior}
self.active_behavior_name = 'zero'
self.set_active_behavior('zero')
self.tf = TransformListener()
self.ddynrec = DDynamicReconfigure("example_dyn_rec")
# Transformers are functions capable of processing incoming data in a variety of formats.
# They are functions that take input of whatever type the topic is, and produce a transformers.Command
# object.
alt_pid = transformers.PIDAltController(self.tf, self.ddynrec)
pos_pid = transformers.PIDPosController(self.tf, self.ddynrec, alt_pid)
print pos_pid.cmd_pos
self.transformers = {
'cmd_vel': (Twist, transformers.cmd_vel),
'cmd_takeoff': (Empty, transformers.cmd_takeoff),
'cmd_land': (Empty, transformers.cmd_land),
'cmd_pos': (PoseStamped, pos_pid.cmd_pos),
'cmd_vel_alt': (VelAlt, alt_pid.cmd_vel_alt)
}
""":type : dict[str, (str, (Any) -> transformers.Command)]"""
# Subscribe to the behaviors passed as ROS parameters
starting_behaviors = rospy.get_param('~behaviors', [])
for b in starting_behaviors:
behavior = Behavior(self.process_command, b, freq=20)
behavior.subscribe(self.transformers)
self.behaviors[b] = behavior
# Secondary behaviors are filters that are always active on the Command before it is published.
# Examples include last-minute obstacle avoidance, speed limiters, or arena boundary constraints.
self.secondaries = []
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=0)
self.takeoff_pub = rospy.Publisher('/takeoff', Empty, queue_size=0)
self.land_pub = rospy.Publisher('/land', Empty, queue_size=0)
self.debug_pub = rospy.Publisher('/arbiter/debug',
String,
queue_size=10)
self.active_pub = rospy.Publisher('/arbiter/active_behavior',
String,
queue_size=10)
self.ddynrec.add_variable(
"midbrain_freq", "Frequency of the main control loop for PIDs",
rospy.get_param('~midbrain_freq', 100), 1, 1000)
self.start_ddynrec()
self.null_behavior.handle_message('cmd_vel', Twist())
rospy.sleep(0.5)
rospy.Subscriber('/arbiter/register', RegisterBehavior,
self.handle_register)
rospy.Subscriber('/arbiter/activate_behavior', String,
self.handle_activate)
def start_ddynrec(self):
"""
Helper function to start the ddynamic reconfigure server with a callback
function that updates the self.ddynrec attribute.
"""
def callback(config, level):
"""
A callback function used to as the parameter in the ddynrec.start() function.
This custom callback function updates the state of self.ddynrec so we can
refer to its variables whenever we have access to it.
"""
rospy.loginfo("Received reconf call: " + str(config))
# Update all variables
var_names = self.ddynrec.get_variable_names()
for var_name in var_names:
self.ddynrec.__dict__[var_name] = config[var_name]
return config
self.ddynrec.start(callback)
def handle_activate(self, msg):
"""
ROS subscriber for the activate_behavior topic
:type msg: String
"""
self.set_active_behavior(msg.data)
def set_active_behavior(self, name):
"""
Sets the active behavior, if the provided name is one of the known behaviors.
:type name: str
"""
if name not in self.behaviors:
# TODO: consider automatically registering new behaviors
rospy.logerr('{} does not exist as a behavior!'.format(name))
self.set_active_behavior('zero')
if name != self.active_behavior_name:
self.active_behavior_name = name
# Stop the vehicle
self.process_command(name, 'cmd_vel', Twist())
rospy.loginfo_throttle(1.0,
'{} selected as active behavior'.format(name))
def handle_register(self, req):
"""
ROS subscriber for adding a new behavior to the system.
It is recommended to publish a single latched message to this behavior when a behavior
node starts.
:type req: RegisterBehavior
"""
if req.name in self.behaviors:
rospy.logwarn("Behavior {} already exists".format(req.name))
old = self.behaviors[req.name]
old.stop()
if not req.name:
rospy.logerr("Behavior cannot be created with empty name")
return
behavior = Behavior(self.process_command,
req.name,
freq=self.ddynrec.midbrain_freq if req.fast else 0)
behavior.subscribe(self.transformers)
self.behaviors[req.name] = behavior
rospy.loginfo("Created behavior {}".format(behavior))
def process_command(self, behavior, topic, raw_cmd):
"""
process_command gets called after a message gets received from the currently active behavior.
:param str behavior: The name of the behavior initiating the command
:param str topic: The topic (without namespace) to which the command was sent
:type raw_cmd: ROS message
:return: success
"""
if behavior != self.active_behavior_name:
# Only messages from the active behavior are handled
return False
_, transformer = self.transformers[topic]
# Convert to a transformers.Command
cmd = transformer(raw_cmd) # type: transformers.Command
# Apply secondary behaviors
for func in self.secondaries:
cmd = func(cmd)
# Publish the result to the ROS network
if cmd.takeoff:
self.takeoff_pub.publish(Empty())
vel = Twist()
vel.linear.z = 0.5
self.vel_pub.publish(vel)
elif cmd.land:
self.land_pub.publish(Empty())
vel = Twist()
vel.linear.z = -0.5
self.vel_pub.publish(vel)
else:
self.vel_pub.publish(cmd.vel)
# Log it
self.active_pub.publish(String(behavior))
rospy.loginfo_throttle(
1, "Command published by {}/{}".format(behavior, topic))
return True
def publish_debug(self):
"""
Publishes debug information to the ROS network
:return: None
"""
self.debug_pub.publish(String(str(self.behaviors)))
def run(self):
"""
Main method, publishes debug information and
:return:
"""
r = rospy.Rate(20)
while not rospy.is_shutdown():
self.publish_debug()
try:
r.sleep()
except rospy.ROSTimeMovedBackwardsException:
r = rospy.Rate(20)
class ClickFollower(object):
def __init__(self):
self.config = DDynamicReconfigure()
self.config.add_variable("rate",
"Frequency to republish updates, in hertz",
rospy.get_param('~rate', 10.0), 0.2, 200.0)
self.config.add_variable("height", "Height to fly above base frame",
rospy.get_param('~height', 2.0), 0.0, 5.0)
self.pub = rospy.Publisher('cmd_pos', PoseStamped, queue_size=10)
self.sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped,
self.onmsg)
self.last_msg = PoseStamped()
self.has_msg = False
self.start_ddynrec()
def onmsg(self, msg):
"""
:type msg: PoseStamped
"""
self.last_msg = msg
self.has_msg = True
msg.pose.position.z = self.config.height
msg.header.stamp = rospy.Time(0)
self.pub.publish(msg)
def run(self):
last_rate = None
r = rospy.Rate(
1) # This is dynamically overridden to be the rate dynamic config
while not rospy.is_shutdown():
if self.config.rate != last_rate:
last_rate = self.config.rate
r = rospy.Rate(last_rate)
print "Rate changed to {}".format(last_rate)
if self.has_msg:
# Republish the last received message
self.onmsg(self.last_msg)
r.sleep()
def start_ddynrec(self):
"""
Helper function to start the ddynamic reconfigure server with a callback
function that updates the self.ddynrec attribute.
"""
def callback(config, level):
"""
A callback function used to as the parameter in the ddynrec.start() function.
This custom callback function updates the state of self.ddynrec so we can
refer to its variables whenever we have access to it.
"""
rospy.loginfo("Received reconfigure request: " + str(config))
# Update all variables
var_names = self.config.get_variable_names()
for var_name in var_names:
self.config.__dict__[var_name] = config[var_name]
return config
self.config.start(callback)
class AllPublisher(object):
def __init__(self):
rp = RosPack()
pkgpath = rp.get_path('panda_bridge_ros')
pkgpath += '/config/honda_civic_touring_2016_can_for_cantools.dbc'
self.parser = load_dbc_file(pkgpath)
self.p = Panda()
self.p.set_safety_mode(self.p.SAFETY_ALLOUTPUT)
self.gas_val = 0
self.brake_val = 0
self.steer_val = 0
self.cruise_modifier = 0.0
self.sub_engine = rospy.Subscriber('/joy',
Joy,
self.engine_cb,
queue_size=1)
self.sub_steer = rospy.Subscriber('/steering_amount',
Int16,
self.steer_cb,
queue_size=1)
self.sub_brake = rospy.Subscriber('/braking_amount',
Int16,
self.brake_cb,
queue_size=1)
self.ddr = DDynamicReconfigure("drive_car")
self.ddr.add_variable(
"rate_buttons",
"rate as to which send the button presses for cruise control", 1,
1, 100)
self.add_variables_to_self()
self.ddr.start(self.dyn_rec_callback)
def add_variables_to_self(self):
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__setattr__(var_name, None)
def dyn_rec_callback(self, config, level):
rospy.loginfo("Received reconf call: " + str(config))
# Update all variables
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__dict__[var_name] = config[var_name]
return config
def engine_cb(self, data):
self.cruise_modifier = data.axes[1]
def steer_cb(self, data):
# print("steer_cb: " + str(data))
if data.data > 3840:
self.steer_val = 3840
elif data.data < -3840:
self.steer_val = -3840
self.steer_val = data.data
def brake_cb(self, data):
# print("brake_cb: " + str(data))
if data.data > 1023:
self.brake_val = 1023
elif data.data < 0:
self.brake_val = 0
self.brake_val = data.data
def run(self):
print("Publishing...")
idx_counter = 0
total_cmds_sent = 0
iterations = 0
last_rate = time.time()
while not rospy.is_shutdown():
#print("rate: " + str(self.rate_buttons))
if iterations % (100 / self.rate_buttons) == 0:
if self.cruise_modifier != 0.0:
if self.cruise_modifier > 0.0:
cruise_command = 'accel_res'
elif self.cruise_modifier < 0.0:
cruise_command = 'decel_set'
settings_command = 'none'
print("Sending cruise command: " + cruise_command)
# (xmission_speed, engine_rpm=2000, odometer=3, idx=0):
cmd = create_buttons_command(cruise_command,
settings_command, idx_counter)
print("command is: " + str(cmd))
print("Sending: " + str(cmd) + " (#" +
str(total_cmds_sent) + ") engine modifier val: " +
str(self.cruise_modifier))
self.p.can_send(cmd[0], cmd[2], 1)
cmd = create_steering_control(self.steer_val, idx_counter)[0]
# print("Sending: " + str(cmd) +
# " (#" + str(total_cmds_sent) +
# ") steer val: " + str(self.steer_val))
self.p.can_send(cmd[0], cmd[2], 1)
# (apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
cmd = create_brake_command(self.brake_val, 1, 0, 0, idx_counter)
# print("Sending: " + str(cmd) +
# " (#" + str(total_cmds_sent) +
# ") brake val: " + str(self.brake_val))
self.p.can_send(cmd[0], cmd[2], 1)
idx_counter += 1
idx_counter %= 4
# its wrong, but who cares
total_cmds_sent += 1
time.sleep(0.01)
iterations += 1
class RestClient(object):
def __init__(self, rest_name, ignored_parameters=[]):
self.rest_name = rest_name
self.ignored_parameters = ignored_parameters
self.rest_services = []
self.ddr = None
rospy.init_node(rest_name + '_client', log_level=rospy.INFO)
self.host = rospy.get_param('~host', '')
if not self.host:
rospy.logerr('host is not set')
sys.exit(1)
self._setup_ddr()
def _get_rest_parameters(self):
try:
url = 'http://{}/api/v1/nodes/{}/parameters'.format(
self.host, self.rest_name)
res = requests_retry_session().get(url)
if res.status_code != 200:
rospy.logerr("Getting parameters failed with status code: %d",
res.status_code)
return []
return res.json()
except Exception as e:
rospy.logerr(str(e))
return []
def _set_rest_parameters(self, parameters):
try:
url = 'http://{}/api/v1/nodes/{}/parameters'.format(
self.host, self.rest_name)
res = requests_retry_session().put(url, json=parameters)
j = res.json()
rospy.logdebug("set parameters response: %s",
json.dumps(j, indent=2))
if 'return_code' in j and j['return_code']['value'] != 0:
rospy.logwarn("Setting parameter failed: %s",
j['return_code']['message'])
return []
if res.status_code != 200:
rospy.logerr("Setting parameters failed with status code: %d",
res.status_code)
return []
return j
except Exception as e:
rospy.logerr(str(e))
return []
def _setup_ddr(self):
self.ddr = DDynamicReconfigure(rospy.get_name())
rest_params = [
p for p in self._get_rest_parameters()
if p['name'] not in self.ignored_parameters
]
def enum_method_from_param(p):
if p['type'] != 'string':
return ""
enum_matches = re.findall(r'.*\[(?P<enum>.+)\].*',
p['description'])
if not enum_matches:
return ""
enum_names = [str(e.strip()) for e in enum_matches[0].split(',')]
enum_list = [self.ddr.const(n, 'str', n, n) for n in enum_names]
return self.ddr.enum(enum_list, p['name'] + '_enum')
for p in rest_params:
level = 0
edit_method = enum_method_from_param(p)
if p['type'] == 'int32':
self.ddr.add(p['name'], 'int', level, p['description'],
p['default'], p['min'], p['max'])
elif p['type'] == 'float64':
self.ddr.add(p['name'], 'double', level, p['description'],
p['default'], p['min'], p['max'])
elif p['type'] == 'string':
self.ddr.add(p['name'],
'str',
level,
p['description'],
str(p['default']),
edit_method=edit_method)
elif p['type'] == 'bool':
self.ddr.add(p['name'], 'bool', level, p['description'],
p['default'])
else:
rospy.logwarn("Unsupported parameter type: %s", p['type'])
self.ddr.start(self._dyn_rec_callback)
def _dyn_rec_callback(self, config, level):
rospy.logdebug("Received reconf call: " + str(config))
new_rest_params = [{
'name': n,
'value': config[n]
} for n in self.ddr.get_variable_names() if n in config]
if new_rest_params:
returned_params = self._set_rest_parameters(new_rest_params)
for p in returned_params:
if p['name'] not in config:
rospy.logerr("param %s not in config", p['name'])
continue
config[p['name']] = p['value']
return config
def call_rest_service(self, name, srv_type=None, request=None):
try:
args = {}
if request is not None:
# convert ROS request to JSON (with custom API mappings)
args = convert_ros_message_to_dictionary(request)
rospy.logdebug('calling {} with args: {}'.format(name, args))
url = 'http://{}/api/v1/nodes/{}/services/{}'.format(
self.host, self.rest_name, name)
res = requests_retry_session().put(url, json={'args': args})
j = res.json()
rospy.logdebug("{} rest response: {}".format(
name, json.dumps(j, indent=2)))
rc = j['response'].get('return_code')
if rc is not None and rc['value'] < 0:
rospy.logwarn("service {} returned an error: [{}] {}".format(
name, rc['value'], rc['message']))
# convert to ROS response
if srv_type is not None:
response = convert_dictionary_to_ros_message(
srv_type._response_class(),
j['response'],
strict_mode=False)
else:
response = j['response']
except Exception as e:
rospy.logerr(str(e))
if srv_type is not None:
response = srv_type._response_class()
if hasattr(response, 'return_code'):
response.return_code.value = -1000
response.return_code.message = str(e)
return response
def add_rest_service(self, srv_type, srv_name, callback):
"""create a service and inject the REST-API service name"""
srv = rospy.Service(rospy.get_name() + "/" + srv_name, srv_type,
partial(callback, srv_name, srv_type))
self.rest_services.append(srv)
class LaserPublisher(object):
def __init__(self):
if not rospy.core.is_initialized():
rospy.init_node('laser_test')
rospy.loginfo("Initialised rospy node: laser_test")
self.tl = TransformListener()
self.lp = LaserProjection()
# Publishers
self.all_laser_pub = rospy.Publisher('/pepper/laser_2',
LaserScan,
queue_size=1)
self.pc_pub = rospy.Publisher('/cloud', PointCloud2, queue_size=1)
self.pcl_pub = rospy.Publisher('/cloudl', PointCloud2, queue_size=1)
self.pcr_pub = rospy.Publisher('/cloudr', PointCloud2, queue_size=1)
self.pc_redone_pub = rospy.Publisher('/cloud_redone',
PointCloud2,
queue_size=1)
self.pc_rereprojected_pub = rospy.Publisher('/cloud_rereprojected',
PointCloud2,
queue_size=1)
# Subscribers
left_sub = Subscriber('/pepper/scan_left', LaserScan)
front_sub = Subscriber('/pepper/scan_front', LaserScan)
right_sub = Subscriber('/pepper/scan_right', LaserScan)
self.ts = TimeSynchronizer([left_sub, front_sub, right_sub], 10)
rospy.loginfo("Finished intialising")
self.ddr = DDR('increment')
default_increment = radians(120.0 * 2.0) / 61.0
self.ddr.add_variable('angle_increment', '', default_increment, 0.05,
0.08)
# 130.665
self.ddr.add_variable('half_max_angle', '', 120., 115., 145.0)
self.ddr.start(self.dyn_rec_callback)
self.ts.registerCallback(self.scan_cb)
rospy.loginfo("Ready to go.")
def add_variables_to_self(self):
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__setattr__(var_name, None)
def dyn_rec_callback(self, config, level):
rospy.loginfo("Received reconf call: " + str(config))
# Update all variables
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__dict__[var_name] = config[var_name]
return config
def scan_cb(self, left, front, right):
rospy.loginfo("We got scan_cb")
translated_points = []
try:
pc_left = self.lp.projectLaser(left, channel_options=0x00)
pc_front = self.lp.projectLaser(front, channel_options=0x00)
pc_right = self.lp.projectLaser(right, channel_options=0x00)
except Exception as e:
rospy.logerr("Failed to transform laser scan because: " + str(e))
pc_left.header.stamp = rospy.Time.now()
pc_left.header.frame_id = 'SurroundingLeftLaser_frame'
self.pcl_pub.publish(pc_left)
self.pcr_pub.publish(pc_right)
transform_right_to_front = self.tl.lookupTransform(
'base_footprint', 'SurroundingRightLaser_frame', rospy.Time.now())
rospy.logdebug("Transform Right to Front:")
rospy.logdebug(transform_right_to_front)
ts = TransformStamped()
ts.transform.translation = Vector3(*transform_right_to_front[0])
ts.transform.rotation = Quaternion(*transform_right_to_front[1])
ts.header.stamp = rospy.Time.now()
transformed_cloud = do_transform_cloud(pc_right, ts)
# right point cloud translation
for p in read_points(transformed_cloud,
field_names=('x', 'y', 'z'),
skip_nans=False):
translated_points.append(p)
for i in range(8):
translated_points.append(
(float('nan'), float('nan'), float('nan')))
transform_front_to_front = self.tl.lookupTransform(
'base_footprint', 'SurroundingFrontLaser_frame', rospy.Time.now())
rospy.logdebug("Transform Front to Front:")
rospy.logdebug(transform_front_to_front)
ts = TransformStamped()
ts.transform.translation = Vector3(*transform_front_to_front[0])
ts.transform.rotation = Quaternion(*transform_front_to_front[1])
ts.header.stamp = rospy.Time.now()
transformed_cloud_f = do_transform_cloud(pc_front, ts)
# front point cloud
for p in read_points(transformed_cloud_f,
field_names=('x', 'y', 'z'),
skip_nans=False):
translated_points.append(p)
transform_left_to_front = self.tl.lookupTransform(
'base_footprint', 'SurroundingLeftLaser_frame', rospy.Time.now())
rospy.logdebug("Transform Left to Front:")
rospy.logdebug(transform_left_to_front)
ts = TransformStamped()
ts.transform.translation = Vector3(*transform_left_to_front[0])
ts.transform.rotation = Quaternion(*transform_left_to_front[1])
ts.header.stamp = rospy.Time.now()
from copy import deepcopy
transformed_cloud_l = do_transform_cloud(deepcopy(pc_left), ts)
for i in range(8):
translated_points.append(
(float('nan'), float('nan'), float('nan')))
# left pc translation
for p in read_points(transformed_cloud_l,
field_names=('x', 'y', 'z'),
skip_nans=False):
translated_points.append(p)
# Create a point cloud from the combined points wrt the front
# laser frame
pc_front.header.frame_id = 'base_footprint'
point_cloud = create_cloud_xyz32(pc_front.header, translated_points)
self.pc_pub.publish(point_cloud)
rospy.logdebug("pointcloud all together len: " +
str(point_cloud.width))
# # double check we have the same thing
# compare_str = "\n"
# for idx, (tp, pcp) in enumerate(zip(translated_points, read_points(point_cloud))):
# compare_str += str(idx).zfill(2) + ":\n"
# compare_str += " tp : " + str(tp)
# compare_str += "\n pcp: " + str(pcp) + "\n"
# rospy.loginfo(compare_str)
# # OK we know they are the same
# # translated_points and point_cloud contain virtually the same data
# Convert combined point cloud into LaserScan
all_laser_msg = LaserScan()
laser_ranges, angle_min, angle_max, angle_increment = self.pc_to_laser(
point_cloud)
all_laser_msg.header.frame_id = 'base_footprint'
all_laser_msg.header.stamp = rospy.Time.now()
all_laser_msg.ranges = laser_ranges
all_laser_msg.angle_min = angle_min
all_laser_msg.angle_max = angle_max
all_laser_msg.angle_increment = angle_increment
all_laser_msg.range_min = 0.1
all_laser_msg.range_max = 7.0
all_laser_msg.intensities = []
self.all_laser_pub.publish(all_laser_msg)
rospy.logdebug("all_laser_msg len: " + str(len(all_laser_msg.ranges)))
pc_redone = self.lp.projectLaser(all_laser_msg, channel_options=0x00)
rospy.logdebug("all_laser pc_redone len: " + str(pc_redone.width))
self.pc_redone_pub.publish(pc_redone)
# compare what came in and what came out
rospy.logdebug("point_cloud frame_id, pc_redone frame_id:")
rospy.logdebug(
(point_cloud.header.frame_id, pc_redone.header.frame_id))
rospy.logdebug("point_cloud is correct, pc_redone is incorrect")
compare_str = "\n"
for idx, (point_in, point_out) in enumerate(
zip(read_points(point_cloud), read_points(pc_redone))):
point_out = [point_out[0], point_out[1], 0.0]
point_in = [point_in[0], point_in[1], 0.0]
compare_str += str(idx).zfill(2) + ":\n"
compare_str += " in : " + str(point_in)
compare_str += "\n out: " + str(point_out) + "\n"
dist = np.linalg.norm(np.array(point_out) - np.array(point_in))
compare_str += " dist: " + str(dist) + "\n"
# angle
angle1 = atan2(point_in[1], point_in[0])
angle2 = atan2(point_out[1], point_out[0])
angle_dif = angle2 - angle1
compare_str += " angle dif: " + str(angle_dif) + "\n"
rospy.logdebug(compare_str)
def pc_to_laser(self, cloud):
laser_points = []
points_rereprojected = []
multiply_num_rays = 8
num_rays = 61 * multiply_num_rays
laser_points2 = [float('nan')] * num_rays
min_angle = -radians(self.half_max_angle)
max_angle = radians(self.half_max_angle)
# angle_increment = self.angle_increment
angle_increment = (radians(self.half_max_angle) *
2.0) / float(num_rays)
big_str = "\n"
for idx, p in enumerate(read_points(cloud, skip_nans=False)):
#dist = self.get_dist(p[0], p[1])
p = [p[0], p[1], 0.0]
dist = np.linalg.norm(np.array((0., 0., 0.)) - np.array(p))
# dist1 = self.get_dist(p[0], p[1])
big_str += " " + str(idx).zfill(2) + ": x: " + str(round(
p[0], 2)) + ", y: " + str(round(p[1], 2)) + ", z: " + str(
round(p[2], 2)) + " = " + str(round(
dist, 2)) + "m (at " + str(
round(degrees(idx * angle_increment + min_angle),
2)) + "deg)\n"
laser_points.append(dist)
# coords from dist
x = dist * cos(idx * angle_increment + min_angle)
y = dist * sin(idx * angle_increment + min_angle)
print(" [ px, py, are the correct points ] ")
print("dist, px, py: " + str(dist) + " " + str(p[0])) + " " + str(
p[1])
print("dist, x, y: " + str(dist) + " " + str(x) + " " + str(y))
dist_from_rereproj = self.get_dist(x, y)
print("dist rereproj: " + str(dist_from_rereproj))
# print("dist1 : " + str(dist1))
# what if a make a pointcloud based in the cos sin version
points_rereprojected.append((x, y, 0.0))
# angle from point
angle = atan2(p[1], p[0])
# angle2 = atan2(y, x)
expected_angle = idx * self.angle_increment + min_angle
if not isnan(angle):
tmp_angle = angle - min_angle
print("tmp_angle: " + str(degrees(tmp_angle))) + " deg"
print("angle_increment: " + str(degrees(angle_increment)))
closest_index = int(tmp_angle / angle_increment)
print("closest index: " + str(closest_index))
if closest_index >= len(laser_points2):
laser_points2[-1] = dist
elif closest_index < 0:
laser_points2[0] = dist
else:
laser_points2[closest_index] = dist
else:
print("nan, not adding anything to scan")
# laser_points[]
print("Angle from p : " + str(round(degrees(angle), 2)))
# print("Angle from xy: " + str(round(degrees(angle2), 2)))
print("Expected angle: " + str(round(degrees(expected_angle), 2)))
rospy.logdebug("Lasered cloud")
rospy.logdebug(big_str)
laser_points = laser_points2
print("Len of laser points after new technique: " +
str(len(laser_points)))
rereprojected_pc = PointCloud2()
rereprojected_pc.header.frame_id = 'base_footprint'
rereprojected_pc.header.stamp = rospy.Time.now()
point_cloud_rere = create_cloud_xyz32(rereprojected_pc.header,
points_rereprojected)
self.pc_rereprojected_pub.publish(point_cloud_rere)
return laser_points, min_angle, max_angle, angle_increment
def get_dist(self, x0, y0, x1=0.0, y1=0.0):
return sqrt((x1 - x0)**2 + (y1 - y0)**2)