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 TwistStampedGains(object):
def __init__(self):
self.pub = rospy.Publisher('/twist_raw', TwistStamped, queue_size=1)
self.ddr = DDynamicReconfigure('twist_gains')
self.ddr.add_variable('linear_x_gain',
"gain for twist.linear.x",
1.0,
min=-5.0,
max=5.0)
self.ddr.add_variable('angular_z_gain',
"gain for twist.angular.z",
25.0,
min=0.0,
max=100.0)
self.ddr.add_variable("max_angular_z",
"max +- angular.z",
6.0,
min=0.0,
max=20.0)
self.ddr.start(self.dyn_rec_callback)
self.sub = rospy.Subscriber('/twist_raw_tmp',
TwistStamped,
self._cb,
queue_size=1)
rospy.loginfo("TwistStampedGains initialized.")
rospy.loginfo("Listening to topic: " + str(self.sub.resolved_name))
rospy.loginfo("Publishing on topic: " + str(self.pub.resolved_name))
rospy.loginfo("Applying gains: x: " + str(self.linear_x_gain) +
" y: " + str(self.angular_z_gain))
def dyn_rec_callback(self, config, level):
self.linear_x_gain = config['linear_x_gain']
self.angular_z_gain = config['angular_z_gain']
self.max_z = config['max_angular_z']
rospy.loginfo("TwistStampedGains got a reconfigure callback: " +
str(config))
return config
def _cb(self, msg):
# rospy.loginfo("in z: " + str(msg.twist.angular.z))
msg.twist.linear.x *= self.linear_x_gain
msg.twist.angular.z *= self.angular_z_gain
if abs(msg.twist.angular.z) > self.max_z:
if msg.twist.angular.z >= 0.0:
msg.twist.angular.z = self.max_z
else:
msg.twist.angular.z = -self.max_z
# rospy.loginfo("out z: " + str(msg.twist.angular.z))
self.pub.publish(msg)
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 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)
def callback(config, level):
#rospy.loginfo("Received reconf call: " + str(config))
return config
if __name__ == '__main__':
rospy.loginfo("Sprint Parameters - Running")
rospy.init_node('sprint_params')
# Create a D(ynamic)DynamicReconfigure
player = DDynamicReconfigure("player")
# Add variables (name, description, default value, min, max, edit_method)
# ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
player.add_variable("Pan_Kp", "Pan Kp", 0.0, 0, 1)
player.add_variable("Pan_Ki", "Pan Ki", 0.0, 0, 1)
player.add_variable("Pan_Kd", "Pan Kd", 0.0, 0, 0.000010)
player.add_variable("Tilt_Kp", "Tilt Kp", 0.0, 0, 1)
player.add_variable("Tilt_Ki", "Tilt Ki", 0.0, 0, 1)
player.add_variable("Tilt_Kd", "Tilt Kd", 0.0, 0, 0.000010)
player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2)
player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5)
player.add_variable("Scan_Rate", "Scan Rate", 0, 0, 10)
player.add_variable("Body_Fwd_Kp", "Body Fwd Kp", 0.0, 0, 5)
player.add_variable("Body_Bwd_Kp", "Body Bwd Kp", 0.0, 0, 1)
# vision = DDynamicReconfigure("vision")
# vision.add_variable("Vote", "Accumulator", 0, 0, 100)
# vision.add_variable("Min_Length", "Min Length", 0, 0, 300)
# vision.add_variable("Max_Gap", "Max Gap", 0, 0, 300)
def setup_ddrec_server():
params_file = c["param_path"]
def dyn_rec_callback(config, level):
param_names = [
# Core Ball
"H_Low_Core",
"H_High_Core",
"S_Low_Core",
"S_High_Core",
"V_Low_Core",
"V_High_Core",
"H_Low_Core",
# Edge Ball
"H_Low_Edge",
"H_High_Edge",
"S_Low_Edge",
"S_High_Edge",
"V_Low_Edge",
"V_High_Edge",
# Angle
"angle_offset"
]
try:
if get_dy("SaveToJson") == 1:
new_params = {param: get_dy(param) for param in param_names}
with open(params_file, 'w') as f:
json.dump(new_params, f)
rospy.loginfo("Parameters saved to json")
except KeyError:
rospy.loginfo("Param not found, is parameter server running?")
return config
dd = DDynamicReconfigure("cv_dyn_rec")
with open(params_file, 'r') as f:
params = json.load(f)
# In Range for central ball
dd.add_variable("H_Low_Core", "Lower bound hue for central ball",
params["H_Low_Core"], 0, 180)
dd.add_variable("H_High_Core", "Higher bound hue for central ball",
params["H_High_Core"], 0, 180)
dd.add_variable("S_Low_Core", "Lower bound saturation for central ball",
params["S_Low_Core"], 0, 255)
dd.add_variable("S_High_Core", "Higher bound saturation for central ball",
params["S_High_Core"], 0, 255)
dd.add_variable("V_Low_Core", "Lower bound value for central ball",
params["V_Low_Core"], 0, 255)
dd.add_variable("V_High_Core", "Higher bound value for central ball",
params["V_High_Core"], 0, 255)
# In Range for edge ball
dd.add_variable("H_Low_Edge", "Lower bound hue for edge ball",
params["H_Low_Edge"], 0, 180)
dd.add_variable("H_High_Edge", "Higher bound hue for edge ball",
params["H_High_Edge"], 0, 180)
dd.add_variable("S_Low_Edge", "Lower bound saturation for edge ball",
params["S_Low_Edge"], 0, 255)
dd.add_variable("S_High_Edge", "Higher bound saturation for edge ball",
params["S_High_Edge"], 0, 255)
dd.add_variable("V_Low_Edge", "Lower bound value for edge ball",
params["V_Low_Edge"], 0, 255)
dd.add_variable("V_High_Edge", "Higher bound value for edge ball",
params["V_High_Edge"], 0, 255)
# Angle offset
dd.add_variable("angle_offset",
"Offset [rad] to added to angle such that hanging "
"pendulum yields an angle of zero",
params["angle_offset"], -3.142, 3.142)
dd.add_variable("SaveToJson", "Evoke callback", 0, 0, 1)
dd.start(dyn_rec_callback)
return dd
class AreaDetector(object):
"""
Obstacle detector, capable of checking if there is an object in a specified area
"""
def __init__(self, name, x_offset, y_offset, x_size, y_size):
# self.n_measurements_consider_valid = 4
self.pub_avoidance_area = rospy.Publisher("/avoidance_area_" + name,
PolygonStamped,
queue_size=1,
latch=True)
self.attention_wide = [x_offset, y_offset] #[1.5, 10.0]
self.attention_center = [x_size, y_size, 0.0] #[-48.9, 120.0, 0.0]
self.attention_area = DDynamicReconfigure("avoidance_aerea" + name)
self.attention_area.add_variable('x_offset',
'x_offset',
self.attention_center[0],
min=self.attention_center[0] - 25.0,
max=self.attention_center[0] + 25.0)
self.attention_area.add_variable('y_offset',
'y_offset',
self.attention_center[1],
min=self.attention_center[1] - 25.0,
max=self.attention_center[1] + 25.0)
self.attention_area.add_variable('x_size',
'x_size',
self.attention_wide[0],
min=0.0,
max=self.attention_wide[0] + 20.0)
self.attention_area.add_variable('y_size',
'y_size',
self.attention_wide[1],
min=0.0,
max=self.attention_wide[1] + 5.0)
self.publish_attention_area()
self.attention_area.start(self.attention_changes_cb)
self.points_found = None
def attention_changes_cb(self, config, level):
self.attention_center[0] = config['x_offset']
self.attention_center[1] = config['y_offset']
self.attention_wide[0] = config['x_size']
self.attention_wide[1] = config['y_size']
self.publish_attention_area()
return config
def publish_attention_area(self):
msg = PolygonStamped()
msg.header.frame_id = "/map"
p1 = Point()
p1.x = self.attention_center[0] + self.attention_wide[0]
p1.y = self.attention_center[1] + self.attention_wide[1]
p1.z = self.attention_center[2]
msg.polygon.points.append(p1)
p2 = Point()
p2.x = self.attention_center[0] + self.attention_wide[0]
p2.y = self.attention_center[1] - self.attention_wide[1]
p2.z = self.attention_center[2]
msg.polygon.points.append(p2)
p3 = Point()
p3.x = self.attention_center[0] - self.attention_wide[0]
p3.y = self.attention_center[1] - self.attention_wide[1]
p3.z = self.attention_center[2]
msg.polygon.points.append(p3)
p4 = Point()
p4.x = self.attention_center[0] - self.attention_wide[0]
p4.y = self.attention_center[1] + self.attention_wide[1]
p4.z = self.attention_center[2]
msg.polygon.points.append(p4)
self.pub_avoidance_area.publish(msg)
return
def check_point(self, x, y, z):
# Checks if a point is on the area
px = x - self.attention_center[0]
py = y - self.attention_center[1]
if (-self.attention_wide[0] < px and px < self.attention_wide[0]):
if (-self.attention_wide[1] < py and py < self.attention_wide[1]):
if self.points_found == None:
self.points_found = []
# The point is inside the box
self.points_found.append([x, y, z])
return
def get_filtered_position(self):
# Centeres the position of the measurements
if self.points_found == None:
return
px = 0
py = 0
pz = 0
for p in self.points_found:
px += p[0]
py += p[1]
pz += p[2]
px /= len(self.points_found)
py /= len(self.points_found)
pz /= len(self.points_found)
return [px, py, pz]
def get_n_points(self):
if self.points_found == None:
return 0
return len(self.points_found)
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)
def callback(config, level):
#rospy.loginfo("Received reconf call: " + str(config))
return config
if __name__ == '__main__':
rospy.loginfo("Marathon Parameters - Running")
rospy.init_node('marathon_params')
# Create a D(ynamic)DynamicReconfigure
player = DDynamicReconfigure("player")
# Add variables (name, description, default value, min, max, edit_method)
# ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
player.add_variable("Pan_Kp", "Pan Kp", 0.0, 0, 1)
player.add_variable("Pan_Ki", "Pan Ki", 0.0, 0, 1)
player.add_variable("Pan_Kd", "Pan Kd", 0.0, 0, 0.000010)
player.add_variable("Tilt_Kp", "Tilt Kp", 0.0, 0, 1)
player.add_variable("Tilt_Ki", "Tilt Ki", 0.0, 0, 1)
player.add_variable("Tilt_Kd", "Tilt Kd", 0.0, 0, 0.000010)
player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2)
player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5)
player.add_variable("Tilt_Angle", "Tilt Angle", 0.0, -2, 0)
player.add_variable("Scan_Rate", "Scan Rate", 0, 0, 10)
player.add_variable("Body_Kp", "Body Kp", 0.0, 0, 5)
# # Yellow
# lyellow = DDynamicReconfigure("line/yellow")
# lyellow.add_variable("H_Max", "H Max", 255, 0, 255)
# lyellow.add_variable("H_Min", "H Min", 0, 0, 255)
class GripperGrasper(object):
def __init__(self):
rospy.loginfo("Initializing GripperGrasper...")
# This node Dynamic params
self.ddr = DDynamicReconfigure("grasper")
self.max_position_error = self.ddr.add_variable("max_position_error",
"Max absolute value of controller state of any joint to stop closing",
0.002, 0.00001, 0.045)
self.timeout = self.ddr.add_variable("timeout",
"timeout for the closing action",
5.0, 0.0, 30.0)
self.closing_time = self.ddr.add_variable("closing_time",
"Time for the closing goal",
2.0, 0.01, 30.0)
self.rate = self.ddr.add_variable("rate",
"Rate Hz at which the node closing will do stuff",
5, 1, 50)
self.ddr.start(self.ddr_cb)
rospy.loginfo("Initialized dynamic reconfigure on: " + str(rospy.get_name()))
# Subscriber to the gripper state
self.last_state = None
self.state_sub = rospy.Subscriber('/gripper_controller/state',
JointTrajectoryControllerState,
self.state_cb,
queue_size=1)
rospy.loginfo("Subscribed to topic: " + str(self.state_sub.resolved_name))
# Publisher on the gripper command topic
self.cmd_pub = rospy.Publisher('/gripper_controller/command',
JointTrajectory,
queue_size=1)
rospy.loginfo("Publishing to topic: " + str(self.cmd_pub.resolved_name))
# Grasping service to offer
self.grasp_srv = rospy.Service('/gripper_controller/grasp',
Empty,
self.grasp_cb)
rospy.loginfo("Offering grasp service on: " + str(self.grasp_srv.resolved_name))
rospy.loginfo("Done initializing GripperGrasper!")
def ddr_cb(self, config, level):
self.max_position_error = config['max_position_error']
self.timeout = config['timeout']
self.closing_time = config['closing_time']
self.rate = config['rate']
return config
def state_cb(self, data):
self.last_state = data
def grasp_cb(self, req):
rospy.logdebug("Received grasp request!")
# From wherever we are close gripper
# Keep closing until the error of the state reaches
# max_position_error on any of the gripper joints
# or we reach timeout
initial_time = rospy.Time.now()
closing_amount = [0.0, 0.0]
# Initial command, wait for it to do something
self.send_close(closing_amount)
rospy.sleep(self.closing_time)
r = rospy.Rate(self.rate)
on_optimal_close = False
while not rospy.is_shutdown() and (rospy.Time.now() - initial_time) < rospy.Duration(self.timeout) and not on_optimal_close:
if -self.last_state.error.positions[0] > self.max_position_error:
rospy.logdebug("Over error joint 0...")
closing_amount = self.get_optimal_close()
on_optimal_close = True
elif -self.last_state.error.positions[1] > self.max_position_error:
rospy.logdebug("Over error joint 1...")
closing_amount = self.get_optimal_close()
on_optimal_close = True
self.send_close(closing_amount)
r.sleep()
return EmptyResponse()
def get_optimal_close(self):
optimal_0 = self.last_state.actual.positions[0] - self.max_position_error
optimal_1 = self.last_state.actual.positions[1] - self.max_position_error
return [optimal_0, optimal_1]
def send_close(self, closing_amount):
rospy.loginfo("Closing: " + str(closing_amount))
jt = JointTrajectory()
jt.joint_names = ['gripper_right_finger_joint',
'gripper_left_finger_joint']
p = JointTrajectoryPoint()
p.positions = closing_amount
p.time_from_start = rospy.Duration(self.closing_time)
jt.points.append(p)
self.cmd_pub.publish(jt)
def callback(config, level):
#rospy.loginfo("Received reconf call: " + str(config))
return config
if __name__ == '__main__':
rospy.loginfo("Marathon Parameters - Running")
rospy.init_node('marathon_params')
# Create a D(ynamic)DynamicReconfigure
player = DDynamicReconfigure("player")
# Add variables (name, description, default value, min, max, edit_method)
# ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
player.add_variable("Pan_KP", "Pan KP", 0.0, 0, 1)
player.add_variable("Pan_KI", "Pan KI", 0.0, 0, 1)
player.add_variable("Pan_KD", "Pan KD", 0.0, 0, 0.000010)
player.add_variable("Tilt_KP", "Tilt KP", 0.0, 0, 1)
player.add_variable("Tilt_KI", "Tilt KI", 0.0, 0, 1)
player.add_variable("Tilt_KD", "Tilt KD", 0.0, 0, 0.000010)
player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2)
player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5)
player.add_variable("Tilt_Angle", "Tilt Angle", 0.0, -2, 0)
player.add_variable("Scan_Rate", "Scan Rate", 0, 0, 10)
player.add_variable("Body_KP", "Body KP", 0.0, 0, 1)
line = DDynamicReconfigure("line")
line.add_variable("H_Max", "H Max", 255, 0, 255)
line.add_variable("H_Min", "H Min", 0, 0, 255)
line.add_variable("S_Max", "S Max", 255, 0, 255)
def callback(config, level):
#rospy.loginfo("Received reconf call: " + str(config))
return config
if __name__ == '__main__':
rospy.loginfo("Sprint Parameters - Running")
rospy.init_node('sprint_params')
# Create a D(ynamic)DynamicReconfigure
player = DDynamicReconfigure("player")
# Add variables (name, description, default value, min, max, edit_method)
# ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
player.add_variable("Pan_KP", "Pan KP", 0.0, 0, 1)
player.add_variable("Pan_KI", "Pan KI", 0.0, 0, 1)
player.add_variable("Pan_KD", "Pan KD", 0.0, 0, 0.000010)
player.add_variable("Tilt_KP", "Tilt KP", 0.0, 0, 1)
player.add_variable("Tilt_KI", "Tilt KI", 0.0, 0, 1)
player.add_variable("Tilt_KD", "Tilt KD", 0.0, 0, 0.000010)
player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2)
player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5)
player.add_variable("Scan_Rate", "Scan Rate", 0, 0, 10)
player.add_variable("Body_Forward_KP", "Body Forward KP", 0.0, 0, 1)
player.add_variable("Body_Backward_KP", "Body Backward KP", 0.0, 0, 1)
vision = DDynamicReconfigure("vision")
vision.add_variable("Vote", "Accumulator", 0, 0, 100)
vision.add_variable("Min_Length", "Min Length", 0, 0, 300)
vision.add_variable("Max_Gap", "Max Gap", 0, 0, 300)
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 SignalGenerator(object):
def __init__(self):
self.t0 = rospy.Time.now()
self.timer = None
self.config = None
self.dr_client = None
self.new_server = False
self.pub = None
self.ddr = DDynamicReconfigure("")
self.ddr.add_variable("server", "dynamic reconfigure server", "")
self.ddr.add_variable("param", "dr parameter name", "")
self.ddr.add_variable("topic", "topic name", "signal")
# The dynamic reconfigure probably won't go lower than 0.1,
# 0.01 only works for topics
self.ddr.add_variable("period", "update period", 0.05, 0.01, 10.0)
self.num_freqs = 3
for i in range(self.num_freqs):
si = str(i)
fr = float(i) / float(self.num_freqs)
self.ddr.add_variable("freq" + si, "frequency " + si,
1.0 - fr * fr, 0.0, 10.0)
self.ddr.add_variable("amp" + si, "amplitude " + si, fr, 0.0, 2.0)
self.ddr.add_variable("phase" + si, "phase " + si, 0.0, -math.pi,
math.pi)
self.ddr.add_variable("offset" + si, "offset " + si, 0.0, -5.0,
5.0)
# TODO(lucasw) sine, sawtooth, ramp, square
self.ddr.start(self.config_callback)
def is_changed(self, config, param):
if self.config is None:
return True
return config[param] != self.config[param]
def config_callback(self, config, level):
if self.is_changed(config, 'period'):
if self.timer:
self.timer.shutdown()
self.timer = rospy.Timer(rospy.Duration(config.period),
self.update)
if self.is_changed(config, 'server'):
self.new_server = True
if self.is_changed(config, 'topic'):
self.pub = rospy.Publisher(config.topic, Float32, queue_size=2)
self.config = config
return config
def server_dr_callback(self, config):
rospy.logdebug(config)
def safe_update_config(self, values):
if self.dr_client is None:
return False
update_timeout = 1.0
# TODO(lucasw) could follow
# https://stackoverflow.com/questions/2829329/catch-a-threads-exception-in-the-caller-thread-in-python
# and pass a message back if the update configuration fails
try:
th1 = threading.Thread(target=self.dr_client.update_configuration,
args=[values])
th1.start()
t1 = rospy.Time.now()
while ((rospy.Time.now() - t1).to_sec() < update_timeout):
if th1.isAlive():
rospy.sleep(0.05)
else:
break
if th1.isAlive():
# TODO(lucasw) how to kill t1- or does it matter?
raise RuntimeError("timeout")
except RuntimeError as ex:
# self.dr_client = None
rospy.logerr(ex + " " + str(self.config.server) + " " +
self.config.param)
return False
return True
def connect_server(self):
if self.config.server == '':
self.dr_client = None
self.new_server = False
return True
try:
self.dr_client = Client(self.config.server,
timeout=0.05,
config_callback=self.server_dr_callback)
self.new_server = False
rospy.loginfo("connected to new server '" + self.config.server +
"'")
except Exception as ex:
rospy.logerr_throttle(
5.0, "no server available '" + self.config.server + "'")
return False
return True
def update(self, event):
if self.config is None:
return
dt = (event.current_real - self.t0).to_sec()
val = 0.0
for i in range(self.num_freqs):
si = str(i)
# TODO(lucasw) want to smoothly transition when
# frequencies change, to do that need to offset phase in a way
# that puts the old frequency and the new one at the same value.
theta = 2.0 * math.pi * self.config[
"freq" + si] * dt + self.config["phase" + si]
val += self.config["amp" + si] * math.sin(theta) + self.config[
"offset" + si]
if self.pub is not None:
self.pub.publish(Float32(val))
if self.new_server:
self.connect_server()
self.safe_update_config({self.config.param: val})
class PedestrianDetector(object):
"""
Pedestrian detector, capable of checking if there is one person un a specific area
"""
def __init__(self):
self.n_measurements_consider_valid = 4
# Publishes the area where the pedestrian pass is
self.pub_pedestrian_area = rospy.Publisher("/pedestrian_area_reference", PolygonStamped, queue_size=1, latch=True)
self.pedestrian_center = [42.3,78.9, -0.48]
self.pedestrian_wide = [6.5,4.0]
self.publish_pedestrian_area()
self.pedestrian_area = DDynamicReconfigure("pedestrian_area")
self.pedestrian_area.add_variable('x_pose','x_pose',
self.pedestrian_center[0],
min= self.pedestrian_center[0] - 1,
max = self.pedestrian_center[0] + 1)
self.pedestrian_area.add_variable('y_pose','y_pose',
self.pedestrian_center[1],
min= self.pedestrian_center[1] - 1,
max = self.pedestrian_center[1] + 1)
self.pedestrian_area.add_variable('x_size','x_size',
self.pedestrian_wide[0],
min= self.pedestrian_wide[0] - 1,
max = self.pedestrian_wide[0] + 1)
self.pedestrian_area.add_variable('y_size','y_size',
self.pedestrian_wide[1],
min= self.pedestrian_wide[1] - 1,
max = self.pedestrian_wide[1] + 1)
self.pub_pedestrian_area_attention = rospy.Publisher("/pedestrian_area_attention", PolygonStamped, queue_size=1, latch=True)
self.pedestrian_recog_area_c = [self.pedestrian_center[0] + self.pedestrian_wide[0]/2.0,
self.pedestrian_center[1] + self.pedestrian_wide[1]/2.0]
self.pedestrian_recog_area_wide_x = [-2.4, 3.3]
self.pedestrian_recog_area_wide_y = self.pedestrian_wide[1]/2 + 0.05
self.publish_pedestrian_area_attention()
# Uses as a reference the direction on which the car comes
self.pedestrian_area.add_variable('x_pedest_neg','x_pedestrian_neg',
self.pedestrian_recog_area_wide_x[0],
min= -5.0,
max = 0.0)
self.pedestrian_area.add_variable('x_pedest_pos','x_pedestrian_pos',
self.pedestrian_recog_area_wide_x[1],
min= 0.0,
max = 5.0)
self.pedestrian_area.start(self.pedestrian_changes_cb)
self.pointcloud_sub = rospy.Subscriber('/velodyne_downsampled_xyz',
PointCloud2,
self.velodyne_cb,
queue_size=1)
self.pub_person_found = rospy.Publisher("/pedestrian_detection",
String, queue_size=1)
self.pub_person_found_area = rospy.Publisher("/pedestrian_area_detection", Marker, queue_size=1, latch=True)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def publish_pedestrian_area(self):
msg = PolygonStamped()
msg.header.frame_id = "/map"
p1 = Point()
p1.x = self.pedestrian_center[0]
p1.y = self.pedestrian_center[1]
p1.z = self.pedestrian_center[2]
msg.polygon.points.append(p1)
p2 = Point()
p2.x = self.pedestrian_center[0] + self.pedestrian_wide[0]
p2.y = self.pedestrian_center[1]
p2.z = self.pedestrian_center[2]
msg.polygon.points.append(p2)
p3 = Point()
p3.x = self.pedestrian_center[0] + self.pedestrian_wide[0]
p3.y = self.pedestrian_center[1] + self.pedestrian_wide[1]
p3.z = self.pedestrian_center[2]
msg.polygon.points.append(p3)
p4 = Point()
p4.x = self.pedestrian_center[0]
p4.y = self.pedestrian_center[1] + self.pedestrian_wide[1]
p4.z = self.pedestrian_center[2]
msg.polygon.points.append(p4)
self.pub_pedestrian_area.publish(msg)
return
def publish_pedestrian_area_attention(self):
msg = PolygonStamped()
msg.header.frame_id = "/map"
p1 = Point()
# Farer away point to th2 factory
p1.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[0]
p1.y = self.pedestrian_recog_area_c[1] + self.pedestrian_recog_area_wide_y
p1.z = self.pedestrian_center[2]
msg.polygon.points.append(p1)
p2 = Point()
p2.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[0]
p2.y = self.pedestrian_recog_area_c[1] - self.pedestrian_recog_area_wide_y
p2.z = self.pedestrian_center[2]
msg.polygon.points.append(p2)
p3 = Point()
# Closest point to the factory
p3.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[1]
p3.y = self.pedestrian_recog_area_c[1] - self.pedestrian_recog_area_wide_y
p3.z = self.pedestrian_center[2]
msg.polygon.points.append(p3)
p4 = Point()
# The point that is at the left of the car
p4.x = self.pedestrian_recog_area_c[0] + self.pedestrian_recog_area_wide_x[1]
p4.y = self.pedestrian_recog_area_c[1] + self.pedestrian_recog_area_wide_y
p4.z = self.pedestrian_center[2]
msg.polygon.points.append(p4)
self.pub_pedestrian_area_attention.publish(msg)
return
def pedestrian_changes_cb(self, config, level):
self.pedestrian_center[0] = config['x_pose']
self.pedestrian_center[1] = config['y_pose']
self.pedestrian_wide[0] = config['x_size']
self.pedestrian_wide[1] = config['y_size']
self.pedestrian_recog_area_wide_x[0] = config['x_pedest_neg']
self.pedestrian_recog_area_wide_x[1] = config['x_pedest_pos']
self.publish_pedestrian_area()
self.publish_pedestrian_area_attention()
return config
def velodyne_cb(self, pointcloud):
# Transforming velodyne to map
transform = self.tf_buffer.lookup_transform("map", pointcloud.header.frame_id,
pointcloud.header.stamp, rospy.Duration(3.0))
pointcloud = do_transform_cloud(pointcloud, transform)
#pointcloud_map
cloud_points = list(read_points(
pointcloud, skip_nans=True, field_names=("x", "y", "z")))
person_found = []
for p in cloud_points:
px = p[0] - self.pedestrian_recog_area_c[0]
py = p[1] - self.pedestrian_recog_area_c[1]
if (abs(py) < self.pedestrian_recog_area_wide_y):
# Taking points at the right of the pedestrian
if (self.pedestrian_recog_area_wide_x[0] < px and
px < self.pedestrian_recog_area_wide_x[1]):
person_found.append([p[0], p[1], p[2]])
if len(person_found) > self.n_measurements_consider_valid:
self.publish_person_found('CROSSING')
self.publish_person_found_area(person_found)
return
self.publish_person_found('CLEAR')
return
def publish_person_found(self, status):
rospy.loginfo('Publishing: ' + status)
self.pub_person_found.publish(status)
def publish_person_found_area(self, position):
# Publishes a circle around the area where the person is placed
msg = PolygonStamped()
msg.header.frame_id = "/map"
px = 0
py = 0
pz = 0
for person in position:
px += person[0]
py += person[1]
pz += person[2]
px /= len(position)
py /= len(position)
pz /= len(position)
r = 0.4
marker = Marker(
type=Marker.CUBE,
id=0,
lifetime=rospy.Duration(0.3),
pose=Pose(Point(px,py,pz), Quaternion(0, 0, 0, 1)),
scale=Vector3(r,r,2.0),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 1.0, 0.0, 0.8),
text="text")
self.pub_person_found_area.publish(marker)
# p.append(Point())
# p4.x = self.pedestrian_center[0]
# p4.y = self.pedestrian_center[1] + self.pedestrian_wide[1]
# p4.z = self.pedestrian_center[2]
# msg.polygon.points.append(p4)
def run(self):
rate = rospy.Rate(10) # Detect at 10hz
while not rospy.is_shutdown():
rate.sleep()
import rospy
from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure
def dyn_rec_callback(config, level):
rospy.loginfo("Received reconf call: " + str(config))
return config
if __name__ == '__main__':
rospy.init_node('test_ddynrec')
# Create a D(ynamic)DynamicReconfigure
ddynrec = DDynamicReconfigure("example_dyn_rec")
# Add variables (name, description, default value, min, max, edit_method)
ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
ddynrec.add_variable("integer", "integer variable", 0, -1, 1)
ddynrec.add_variable("bool", "bool variable", True)
ddynrec.add_variable("string", "string variable", "string dynamic variable")
enum_method = ddynrec.enum([ ddynrec.const("Small", "int", 0, "A small constant"),
ddynrec.const("Medium", "int", 1, "A medium constant"),
ddynrec.const("Large", "int", 2, "A large constant"),
ddynrec.const("ExtraLarge", "int", 3, "An extra large constant")],
"An enum example")
ddynrec.add_variable("enumerate", "enumerate variable", 1, 0, 3, edit_method=enum_method)
# Start the server
ddynrec.start(dyn_rec_callback)
rospy.spin()
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 GripperGraspService(object):
def __init__(self):
rospy.loginfo("Initializing Gripper Grasper...")
# Get the params from param server
self.last_state = None
self.controller_name = rospy.get_param("~controller_name", None)
if not self.controller_name:
rospy.logerr("No controller name found in param: ~controller_name")
exit(1)
self.real_joint_names = rospy.get_param("~real_joint_names", None)
if not self.real_joint_names:
rospy.logerr(
"No real joint names given in param: ~real_joint_names")
exit(1)
self.close_configuration = rospy.get_param("~close_configuration",
None)
if not self.close_configuration:
rospy.logerr(
"No close configuration values given in param: ~close_configuration"
)
exit(1)
if len(self.close_configuration) != len(self.real_joint_names):
rospy.logerr(
"The close configuration values defined in param: ~close_configuration"
", should be of same length as that of joints in param: ~real_joint_names"
)
exit(1)
# This node Dynamic params
self.ddr = DDynamicReconfigure(self.controller_name + "_grasp_service")
self.max_position_error = self.ddr.add_variable(
"max_position_error", "Max absolute value of controller "
"state of any joint to stop closing", 0.002, 0.00001, 0.045)
self.timeout = self.ddr.add_variable("timeout",
"timeout for the closing action",
5.0, 0.0, 30.0)
self.closing_time = self.ddr.add_variable("closing_time",
"Time for the closing goal",
2.0, 0.01, 30.0)
self.rate = self.ddr.add_variable(
"rate", "Rate Hz at which the node closing will do stuff", 5, 1,
50)
self.ddr.start(self.ddr_cb)
rospy.loginfo("Initialized dynamic reconfigure on: " +
str(rospy.get_name()))
# Subscriber to the gripper state
self.state_sub = rospy.Subscriber('/' + self.controller_name +
'_controller/state',
JointTrajectoryControllerState,
self.state_cb,
queue_size=1)
rospy.loginfo("Subscribed to topic: " +
str(self.state_sub.resolved_name))
# Publisher on the gripper command topic
self.cmd_pub = rospy.Publisher('/' + self.controller_name +
'_controller/command',
JointTrajectory,
queue_size=1)
rospy.loginfo("Publishing to topic: " +
str(self.cmd_pub.resolved_name))
# Grasping service to offer
self.grasp_srv = rospy.Service(
'/' + self.controller_name + '_controller/grasp', Empty,
self.grasp_cb)
rospy.loginfo("Offering grasp service on: " +
str(self.grasp_srv.resolved_name))
rospy.loginfo("Done initializing Gripper Grasp Service!")
def ddr_cb(self, config, level):
self.max_position_error = config['max_position_error']
self.timeout = config['timeout']
self.closing_time = config['closing_time']
self.rate = config['rate']
return config
def state_cb(self, data):
self.last_state = data
def grasp_cb(self, req):
rospy.logdebug("Received grasp request!")
# From wherever we are close gripper
# Keep closing until the error of the state reaches
# max_position_error on any of the gripper joints
# or we reach timeout
initial_time = rospy.Time.now()
closing_amount = self.close_configuration
# Initial command, wait for it to do something
self.send_close(closing_amount)
rospy.sleep(self.closing_time)
r = rospy.Rate(self.rate)
on_optimal_close = False
while not rospy.is_shutdown() and \
(rospy.Time.now() - initial_time) < rospy.Duration(self.timeout) and \
not on_optimal_close:
for index in range(len(self.last_state.error.positions)):
if -self.last_state.error.positions[
index] > self.max_position_error:
rospy.logdebug("Over error joint {}...".format(index))
closing_amount = self.get_optimal_close()
on_optimal_close = True
self.send_close(closing_amount)
r.sleep()
return EmptyResponse()
def get_optimal_close(self):
optimal_state = []
for pos in self.last_state.actual.positions:
optimal_state.append(pos - self.max_position_error)
return optimal_state
def send_close(self, closing_amount):
rospy.loginfo("Closing: " + str(closing_amount))
jt = JointTrajectory()
jt.joint_names = self.real_joint_names
p = JointTrajectoryPoint()
p.positions = closing_amount
p.time_from_start = rospy.Duration(self.closing_time)
jt.points.append(p)
self.cmd_pub.publish(jt)
def callback(config, level):
#rospy.loginfo("Received reconf call: " + str(config))
return config
if __name__ == '__main__':
rospy.loginfo("United Soccer Parameters - Running")
rospy.init_node('united_soccer_params')
# Create a D(ynamic)DynamicReconfigure
player = DDynamicReconfigure("player")
# Add variables (name, description, default value, min, max, edit_method)
# ddynrec.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
player.add_variable("Pan_KP", "Pan KP", 0.0, 0, 1)
player.add_variable("Pan_KI", "Pan KI", 0.0, 0, 1)
player.add_variable("Pan_KD", "Pan KD", 0.0, 0, 0.000010)
player.add_variable("Tilt_KP", "Tilt KP", 0.0, 0, 1)
player.add_variable("Tilt_KI", "Tilt KI", 0.0, 0, 1)
player.add_variable("Tilt_KD", "Tilt KD", 0.0, 0, 0.000010)
player.add_variable("Pan_Step", "Pan Step", 0.0, 0, 0.2)
player.add_variable("Tilt_Step", "Tilt Step", 0.0, 0, 0.5)
player.add_variable("Pan_Kick", "Pan Kick", 0.0, -1.5, 0)
player.add_variable("Tilt_Kick", "Tilt Kick", 0.0, -2, 0)
player.add_variable("Body_KP", "Body KP", 0.0, 0, 1)
player.add_variable("KP_Ball_Pos_X", "KP Ball Pos X", 0.0, 0, 1)
player.add_variable("KP_Ball_Pos_Y", "KP Ball Pos Y", 0.0, 0, 1)
player.add_variable("KP_Compass_X", "KP Compass X", 0.0, 0, 1)
player.add_variable("KP_Compass_Y", "KP Compass Y", 0.0, 0, 1)
player.add_variable("KP_Compass_A", "KP Compass A", 0.0, 0, 1)