angshumanG's profile - activity

I am trying to run MPC on Julia and its results (acceleration and steering) I am publishing to a topic. A low level controller then converts it to pwm values for the arduino. Now my problem is the initial few outputs of the MPC has fluctuating frequency due to which I think the servo motor is not working. The servo motor works when it uses constant frequency pwm signals.

rostopic hz /ecu_pwm 
subscribed to [/ecu_pwm]
average rate: 15.874
    min: 0.063s max: 0.063s std dev: 0.00000s window: 2
no new messages
no new messages
average rate: 1.118
    min: 0.063s max: 2.901s std dev: 1.17307s window: 5
average rate: 2.268
    min: 0.045s max: 2.901s std dev: 0.83080s window: 11
average rate: 2.772
    min: 0.045s max: 2.901s std dev: 0.68780s window: 16
average rate: 3.090

How to the stop the topic from publishing or publishing a constant output till the frequency of the MPC stabilizes.

Its just the first 3 -4 runs for ~1 sec which has fluctuating frequency and later on it becomes static.

My code:

#!/usr/bin/env python

# ---------------------------------------------------------------------------
# Licensing Information: You are free to use or extend these projects for
# education or reserach purposes provided that (1) you retain this notice
# and (2) you provide clear attribution to UC Berkeley, including a link
# to http://barc-project.com
#
# Attibution Information: The barc project ROS code-base was developed at UC
# Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales
# (jon.gonzales@berkeley.edu) and Greg Marcil (grmarcil@berkeley.edu). The cloud
# services integation with ROS was developed by Kiet Lam
# (kiet.lam@berkeley.edu). The web-server app Dator was based on an open source
# project by Bruce Wootton
# The original code is being modified by Angshuman Goswami from Clemson University
# working in Efficient Mobility via Connectivity and Control (EMC2) lab
# ---------------------------------------------------------------------------

# README: This node serves as an outgoing messaging bus from odroid to arduino
# Subscribes: steering and motor commands on 'ecu'
# Publishes: combined ecu commands as 'ecu_pwm'

from rospy import init_node, Subscriber, Publisher, get_param
from rospy import Rate, is_shutdown, ROSInterruptException, spin, on_shutdown
from barc.msg import ECU_raw, ECU
from numpy import pi
import rospy

motor_pwm = 90
servo_pwm = 90
str_ang_max = 35
str_ang_min = -35


def pwm_converter_callback(msg):
    global motor_pwm, servo_pwm, b0
    global str_ang_max, str_ang_min

    # translate from SI units in vehicle model
    # to pwm angle units (i.e. to send command signal to actuators)

    # convert desired steering angle to degrees, saturate based on input limits
    str_ang     = max( min( 180.0/pi*msg.servo, str_ang_max), str_ang_min)
    servo_pwm   = 92.0558 + 1.8194*str_ang  - 0.0104*str_ang**2

    # compute motor command
    FxR         =  float(msg.motor) 
    if FxR == 0:
        motor_pwm = 90.0
    elif FxR > 0:
        motor_pwm   =  FxR/b0 + 95.0
    else:
        motor_pwm = 90.0
    update_arduino()

def neutralize():
    global motor_pwm
    motor_pwm = 90
    servo_pwm = 90
    update_arduino()

def update_arduino():
    global motor_pwm, servo_pwm, ecu_pub
    ecu_cmd = ECU(motor_pwm, servo_pwm)
    ecu_pub.publish(ecu_cmd)

def arduino_interface():
    global ecu_pub, b0

    # launch node, subscribe to motorPWM and servoPWM, publish ecu ...

I am trying to subscribe to messages which is being published on a topic (/ecu ) which has the steering and the servo input. I am getting an error [WARN] [WallTime: 1469219387.121113] Serial Port read returned short (expected 8 bytes, received 4 instead). [WARN] [WallTime: 1469219387.121657] Serial Port read failure: my code for publishing to the topic /ecu is :

import rospy
from barc.msg import ECU

def main_auto():
    # initialize ROS node
    rospy.init_node('auto_mode', anonymous=True)
    nh = rospy.Publisher('ecu', ECU, queue_size = 10)
    # set node rate
    rateHz  = 50
    dt      = 1.0 / rateHz
    rate    = rospy.Rate(rateHz)
    t_i     = 0


    # main loop
    while not rospy.is_shutdown():
        # get steering wheel command
        servoCMD = 96.0
        motorCMD = 92.0

    # send command signal
        ecu_cmd = ECU(motorCMD, servoCMD)
        nh.publish(ecu_cmd)

        # wait
        t_i += 1
        rate.sleep()

#############################################################
if __name__ == '__main__':
    try:

        main_auto()
    except rospy.ROSInterruptException:
        pass

the arduino code i have used it from the open source BARC-project

/* ---------------------------------------------------------------------------
# Licensing Information: You are free to use or extend these projects for
# education or reserach purposes provided that (1) you retain this notice
# and (2) you provide clear attribution to UC Berkeley, including a link
# to http://barc-project.com
#
# Attibution Information: The barc project ROS code-base was developed
# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales
# (jon.gonzales@berkeley.edu)  Development of the web-server app Dator was
# based on an open source project by Bruce Wootton, with contributions from
# Kiet Lam (kiet.lam@berkeley.edu). The RC Input code was based on sample code
# from http://rcarduino.blogspot.com/2012/04/how-to-read-multiple-rc-channels-draft.html
# --------------------------------------------------------------------------- */


/* ---------------------------------------------------------------------------
WARNING:
* Be sure to have all ultrasound sensors plugged in, otherwise the pins may get stuck in
  some float voltage
# --------------------------------------------------------------------------- */

// include libraries
#include <ros.h>
#include <barc/Ultrasound.h>
#include <barc/Encoder.h>
#include <barc/ECU.h>
#include <Servo.h>
#include "Maxbotix.h"
#include <EnableInterrupt.h>

/**************************************************************************
CAR CLASS DEFINITION (would like to refactor into car.cpp and car.h but can't figure out arduino build process so far)
**************************************************************************/
class Car {
  public:
    void initEncoders();
    void initRCInput();
    void initActuators();
    void armActuators();
    void writeToActuators(const barc::ECU& ecu);
    // Used for copying variables shared with interrupts to avoid read/write
    // conflicts later
    void readAndCopyInputs();
    // Getters
    uint8_t getRCThrottle();
    uint8_t getRCSteering();
    int getEncoderFL();
    int getEncoderFR();
    int getEncoderBL();
    int getEncoderBR();
    // Interrupt service routines
    void incFR();
    void incFL();
    void incBR();
    void incBL();
    void calcThrottle();
    void calcSteering();
  private:
    // Pin assignments
    const int ENC_FL_PIN = 2;
    const int ENC_FR_PIN = 3;
    const int ENC_BL_PIN = 5;
    const int ENC_BR_PIN = 6;
    const int THROTTLE_PIN = 7;
    const int STEERING_PIN = 8;
    const int MOTOR_PIN = 10;
    const int SERVO_PIN= 11;

    // Car properties
    // unclear what this is for
    const int noAction = 0;

    // Motor limits
    // TODO are these the real limits?
    const int MOTOR_MAX = 120;
    const int MOTOR_MIN = 40;
    const int MOTOR_NEUTRAL = 90;
    // Optional: smaller values for testing safety
    /* const int MOTOR_MAX = 100; */
    /* const int MOTOR_MIN = 75; */

    // Steering limits
    // TODO seems to me that the permissible steering range is really about [58,
    // 120] judging from the sound of the servo pushing beyond a mechanical limit
    // outside that range. The ...