def __init__(self, name, **kwargs):
print('VelocityControllerNode: initializing node')
super().__init__(name, **kwargs)
self.config = {}
self.v_linear_des = numpy.zeros(3)
self.v_angular_des = numpy.zeros(3)
# Initialize pids with default parameters
self.pid_angular = PIDRegulator(1, 0, 0, 1)
self.pid_linear = PIDRegulator(1, 0, 0, 1)
#Declared parameters are overriden with yaml values
self._declare_and_fill_map("linear_p", 1., "p component of pid for linear vel.", self.config)
self._declare_and_fill_map("linear_i", 0.0, "i component of pid for linear vel.", self.config)
self._declare_and_fill_map("linear_d", 0., "d component of pid for linear vel.", self.config)
self._declare_and_fill_map("linear_sat", 1., "saturation of pid for linear vel.", self.config)
self._declare_and_fill_map("angular_p", 1., "p component of pid for angular vel.", self.config)
self._declare_and_fill_map("angular_i", 0.0, "i component of pid for angular vel.", self.config)
self._declare_and_fill_map("angular_d", 0.0, "d component of pid for angular vel.", self.config)
self._declare_and_fill_map("angular_sat", 3.0, "saturation of pid for angular vel.", self.config)
self._declare_and_fill_map(
"odom_vel_in_world", True, "Is odometry velocity supplied in world frame? (gazebo)", self.config)
self.set_parameters_callback(self.callback_params)
self.create_pids(self.config)
# ROS infrastructure
self.sub_cmd_vel = self.create_subscription(geometry_msgs.Twist, 'cmd_vel', self.cmd_vel_callback, 10)
self.sub_odometry = self.create_subscription(Odometry, 'odom', self.odometry_callback, 10)
self.pub_cmd_accel = self.create_publisher( geometry_msgs.Accel, 'cmd_accel', 10)
def create_pids(self, config):
self.pid_linear = PIDRegulator(config['linear_p'], config['linear_i'],
config['linear_d'],
config['linear_sat'])
self.pid_angular = PIDRegulator(config['angular_p'],
config['angular_i'],
config['angular_d'],
config['angular_sat'])
def __init__(self, name, **kwargs):
super().__init__(name, **kwargs)
self.get_logger().info('PositionControllerNode: initializing node')
self.config = {}
self.pos_des = numpy.zeros(3)
self.quat_des = numpy.array([0, 0, 0, 1])
self.initialized = False
# Initialize pids with default parameters
self.pid_rot = PIDRegulator(1, 0, 0, 1)
self.pid_pos = PIDRegulator(1, 0, 0, 1)
self._declare_and_fill_map("pos_p", 1.,
"p component of pid for position",
self.config)
self._declare_and_fill_map("pos_i", 0.0,
"i component of pid for position.",
self.config)
self._declare_and_fill_map("pos_d", 0.0,
"d component of pid for position.",
self.config)
self._declare_and_fill_map("pos_sat", 10.0,
"saturation of pid for position.",
self.config)
self._declare_and_fill_map("rot_p", 1.,
"p component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_i", 0.0,
"i component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_d", 0.0,
"d component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_sat", 3.0,
"saturation of pid for orientation.",
self.config)
self.add_on_set_parameters_callback(self.callback_params)
self.create_pids(self.config)
# ROS infrastructure
self.sub_cmd_pose = self.create_subscription(geometry_msgs.PoseStamped,
'cmd_pose',
self.cmd_pose_callback,
10)
self.sub_odometry = self.create_subscription(Odometry, 'odom',
self.odometry_callback,
10)
self.pub_cmd_vel = self.create_publisher(geometry_msgs.Twist,
'cmd_vel', 10)
def __init__(self, node_name, **kwargs):
print('VelocityControllerNode: initializing node')
super().__init__(node_name, **kwargs)
self.config = {}
self.v_linear_des = numpy.zeros(3)
self.v_angular_des = numpy.zeros(3)
# Initialize pids with default parameters
self.pid_angular = PIDRegulator(1, 0, 0, 1)
self.pid_linear = PIDRegulator(1, 0, 0, 1)
#Declared parameters are overriden with yaml values
self._declare_and_fill_map("linear_p", 1., self.config)
self._declare_and_fill_map("linear_i", 0.0, self.config)
self._declare_and_fill_map("linear_d", 0., self.config)
self._declare_and_fill_map("linear_sat", 1., self.config)
self._declare_and_fill_map("angular_p", 1., self.config)
self._declare_and_fill_map("angular_i", 0.0, self.config)
self._declare_and_fill_map("angular_d", 0.0, self.config)
self._declare_and_fill_map("angular_sat", 3.0, self.config)
self._declare_and_fill_map("odom_vel_in_world", True, self.config)
self.set_parameters_callback(self.callback_params)
self.create_pids(self.config)
# self.pid_linear = PIDRegulator(
# self.config['linear_p'], self.config['linear_i'], self.config['linear_d'], self.config['linear_sat'])
# self.pid_angular = PIDRegulator(
# self.config['angular_p'], self.config['angular_i'], self.config['angular_d'], self.config['angular_sat'])
# self.declare_parameter("linear_p", 1.)
# self.declare_parameter("linear_i", 0.0)
# self.declare_parameter("linear_d", 0.0)
# self.declare_parameter("linear_sat", 10.0)
# self.declare_parameter("angular_p", 1.)
# self.declare_parameter("angular_i", 0.0)
# self.declare_parameter("angular_d", 0.0)
# self.declare_parameter("angular_sat", 3.0)
# self.declare_parameter("odom_vel_in_world", True)
# ROS infrastructure
self.sub_cmd_vel = self.create_subscription(geometry_msgs.Twist,
'cmd_vel',
self.cmd_vel_callback, 10)
self.sub_odometry = self.create_subscription(Odometry, 'odom',
self.odometry_callback,
10)
self.pub_cmd_accel = self.create_publisher(geometry_msgs.Accel,
'cmd_accel', 10)
class VelocityControllerNode(Node):
def __init__(self, node_name, **kwargs):
print('VelocityControllerNode: initializing node')
super().__init__(node_name, **kwargs)
self.config = {}
self.v_linear_des = numpy.zeros(3)
self.v_angular_des = numpy.zeros(3)
# Initialize pids with default parameters
self.pid_angular = PIDRegulator(1, 0, 0, 1)
self.pid_linear = PIDRegulator(1, 0, 0, 1)
#Declared parameters are overriden with yaml values
self._declare_and_fill_map("linear_p", 1.,
"p component of pid for linear vel.",
self.config)
self._declare_and_fill_map("linear_i", 0.0,
"i component of pid for linear vel.",
self.config)
self._declare_and_fill_map("linear_d", 0.,
"d component of pid for linear vel.",
self.config)
self._declare_and_fill_map("linear_sat", 1.,
"saturation of pid for linear vel.",
self.config)
self._declare_and_fill_map("angular_p", 1.,
"p component of pid for angular vel.",
self.config)
self._declare_and_fill_map("angular_i", 0.0,
"i component of pid for angular vel.",
self.config)
self._declare_and_fill_map("angular_d", 0.0,
"d component of pid for angular vel.",
self.config)
self._declare_and_fill_map("angular_sat", 3.0,
"saturation of pid for angular vel.",
self.config)
self._declare_and_fill_map(
"odom_vel_in_world", True,
"Is odometry velocity supplied in world frame? (gazebo)",
self.config)
self.set_parameters_callback(self.callback_params)
self.create_pids(self.config)
# ROS infrastructure
self.sub_cmd_vel = self.create_subscription(geometry_msgs.Twist,
'cmd_vel',
self.cmd_vel_callback, 10)
self.sub_odometry = self.create_subscription(Odometry, 'odom',
self.odometry_callback,
10)
self.pub_cmd_accel = self.create_publisher(geometry_msgs.Accel,
'cmd_accel', 10)
#==============================================================================
def cmd_vel_callback(self, msg):
"""Handle updated set velocity callback."""
# Just store the desired velocity. The actual control runs on odometry callbacks
v_l = msg.linear
v_a = msg.angular
self.v_linear_des = numpy.array([v_l.x, v_l.y, v_l.z])
self.v_angular_des = numpy.array([v_a.x, v_a.y, v_a.z])
#==============================================================================
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
linear = msg.twist.twist.linear
angular = msg.twist.twist.angular
v_linear = numpy.array([linear.x, linear.y, linear.z])
v_angular = numpy.array([angular.x, angular.y, angular.z])
if self.config['odom_vel_in_world']:
# This is a temp. workaround for gazebo's pos3d plugin not behaving properly:
# Twist should be provided wrt child_frame, gazebo provides it wrt world frame
# see http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w])
q_wb = xyzw_array(msg.pose.pose.orientation)
R_bw = transf.quaternion_matrix(q_wb)[0:3, 0:3].transpose()
v_linear = R_bw.dot(v_linear)
v_angular = R_bw.dot(v_angular)
# Compute compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
e_v_linear = (self.v_linear_des - v_linear)
e_v_angular = (self.v_angular_des - v_angular)
a_linear = self.pid_linear.regulate(e_v_linear, t)
a_angular = self.pid_angular.regulate(e_v_angular, t)
# Convert and publish accel. command:
cmd_accel = geometry_msgs.Accel()
cmd_accel.linear = geometry_msgs.Vector3(x=a_linear[0],
y=a_linear[1],
z=a_linear[2])
cmd_accel.angular = geometry_msgs.Vector3(x=a_angular[0],
y=a_angular[1],
z=a_angular[2])
self.pub_cmd_accel.publish(cmd_accel)
#==============================================================================
def callback_params(self, data):
for parameter in data:
self.config[parameter.name] = parameter.value
# config has changed, reset PID controllers
self.create_pids(self.config)
self.get_logger().warn("Parameters dynamically changed...")
return SetParametersResult(successful=True)
#==============================================================================
def create_pids(self, config):
self.pid_linear = PIDRegulator(config['linear_p'], config['linear_i'],
config['linear_d'],
config['linear_sat'])
self.pid_angular = PIDRegulator(config['angular_p'],
config['angular_i'],
config['angular_d'],
config['angular_sat'])
#==============================================================================
def _declare_and_fill_map(self, key, default_value, description, map):
param = self.declare_parameter(
key, default_value, ParameterDescriptor(description=description))
map[key] = param.value
class PositionControllerNode(Node):
def __init__(self, name, **kwargs):
super().__init__(name, **kwargs)
self.get_logger().info('PositionControllerNode: initializing node')
self.config = {}
self.pos_des = numpy.zeros(3)
self.quat_des = numpy.array([0, 0, 0, 1])
self.initialized = False
# Initialize pids with default parameters
self.pid_rot = PIDRegulator(1, 0, 0, 1)
self.pid_pos = PIDRegulator(1, 0, 0, 1)
self._declare_and_fill_map("pos_p", 1.,
"p component of pid for position",
self.config)
self._declare_and_fill_map("pos_i", 0.0,
"i component of pid for position.",
self.config)
self._declare_and_fill_map("pos_d", 0.0,
"d component of pid for position.",
self.config)
self._declare_and_fill_map("pos_sat", 10.0,
"saturation of pid for position.",
self.config)
self._declare_and_fill_map("rot_p", 1.,
"p component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_i", 0.0,
"i component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_d", 0.0,
"d component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_sat", 3.0,
"saturation of pid for orientation.",
self.config)
self.add_on_set_parameters_callback(self.callback_params)
self.create_pids(self.config)
# ROS infrastructure
self.sub_cmd_pose = self.create_subscription(geometry_msgs.PoseStamped,
'cmd_pose',
self.cmd_pose_callback,
10)
self.sub_odometry = self.create_subscription(Odometry, 'odom',
self.odometry_callback,
10)
self.pub_cmd_vel = self.create_publisher(geometry_msgs.Twist,
'cmd_vel', 10)
#==============================================================================
def cmd_pose_callback(self, msg):
"""Handle updated set pose callback."""
# Just store the desired pose. The actual control runs on odometry callbacks
p = msg.pose.position
q = msg.pose.orientation
self.pos_des = numpy.array([p.x, p.y, p.z])
self.quat_des = numpy.array([q.x, q.y, q.z, q.w])
#==============================================================================
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Position error
e_pos_world = self.pos_des - p
e_pos_body = transf.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = numpy.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = transf.quaternion_multiply(
transf.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(x=v_linear[0],
y=v_linear[1],
z=v_linear[2])
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
#==============================================================================
def callback_params(self, data):
"""Handle updated configuration values."""
for parameter in data:
#if parameter.name == "name":
#if parameter.type_ == Parameter.Type.DOUBLE:
self.config[parameter.name] = parameter.value
# Config has changed, reset PID controllers
self.create_pids(self.config)
self.get_logger().warn("Parameters dynamically changed...")
return SetParametersResult(successful=True)
#==============================================================================
def create_pids(self, config):
self.pid_pos = PIDRegulator(config['pos_p'], config['pos_i'],
config['pos_d'], config['pos_sat'])
self.pid_rot = PIDRegulator(config['rot_p'], config['rot_i'],
config['rot_d'], config['rot_sat'])
#==============================================================================
def _declare_and_fill_map(self, key, default_value, description, map):
param = self.declare_parameter(
key, default_value, ParameterDescriptor(description=description))
map[key] = param.value
def create_pids(self, config):
self.pid_pos = PIDRegulator(config['pos_p'], config['pos_i'],
config['pos_d'], config['pos_sat'])
self.pid_rot = PIDRegulator(config['rot_p'], config['rot_i'],
config['rot_d'], config['rot_sat'])
class OrientationControllerNode(Node):
def __init__(self, name, **kwargs):
super().__init__(name, **kwargs)
self.get_logger().info('OrientationControllerNode: initializing node')
self.config = {}
self.quat_des = numpy.array([0, 0, 0, 1])
self.initialized = False
# Initialize pids with default parameters
self.pid_rot = PIDRegulator(1, 0, 0, 1)
self._declare_and_fill_map("rot_p", 1.,
"p component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_i", 0.0,
"i component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_d", 0.0,
"d component of pid for orientation.",
self.config)
self._declare_and_fill_map("rot_sat", 3.0,
"saturation of pid for orientation.",
self.config)
self.set_parameters_callback(self.callback_params)
self.create_pids(self.config)
# ROS infrastructure
self.sub_cmd_pose = self.create_subscription(geometry_msgs.PoseStamped,
'cmd_pose',
self.cmd_pose_callback,
10)
self.sub_odometry = self.create_subscription(Imu, 'imu',
self.odometry_callback,
10)
self.pub_cmd_vel = self.create_publisher(geometry_msgs.Twist,
'cmd_vel', 10)
#==============================================================================
def cmd_pose_callback(self, msg):
"""Handle updated set pose callback."""
# Just store the desired pose. The actual control runs on odometry callbacks
q = msg.pose.orientation
self.quat_des = numpy.array([q.x, q.y, q.z, q.w])
print("orientation is {}".format(q))
#==============================================================================
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
q = msg.orientation
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
#==============================================================================
def callback_params(self, data):
"""Handle updated configuration values."""
for parameter in data:
#if parameter.name == "name":
#if parameter.type_ == Parameter.Type.DOUBLE:
self.config[parameter.name] = parameter.value
# Config has changed, reset PID controllers
self.create_pids(self.config)
self.get_logger().warn("Parameters dynamically changed...")
return SetParametersResult(successful=True)
#==============================================================================
def create_pids(self, config):
self.pid_rot = PIDRegulator(config['rot_p'], config['rot_i'],
config['rot_d'], config['rot_sat'])
#==============================================================================
def _declare_and_fill_map(self, key, default_value, description, map):
param = self.declare_parameter(
key, default_value, ParameterDescriptor(description=description))
map[key] = param.value