コード例 #1
0
ファイル: velocity_control.py プロジェクト: jvitorsn/Plankton
    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)
コード例 #2
0
 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'])
コード例 #3
0
    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)
コード例 #4
0
    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)
コード例 #5
0
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
コード例 #6
0
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
コード例 #7
0
 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'])
コード例 #8
0
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