How to subscribe information from terminal to microprocessor regularly?

asked 2018-11-28 10:10:20 -0600

Alan278 gravatar image

updated 2018-11-28 13:43:21 -0600

jayess gravatar image

I write a code which was upload to microprocessor and I would like to control the message which is subsrcibe from terminal. On terminal I run

roscore

after it I run rosserial client

rosrun rosserial_python serial_node.py /dev/ttyUSB0

And I watching information which is published

rostopic echo chatter

After it I send data to microprocessor

rostopic pub servo std_msgs/UInt16  <information>  --once

and I am doing it few times for different value. The problem is that the information which i send to microprocessor from terminal is updated only during reset microprocessor. I would like to control servos and I need to have to do it in the realtime. Please help me. This is my code which I uploaded to microprocessor :

XYZrobotServo servo1(servoSerial1, 0x01);
XYZrobotServo servo2(servoSerial1, 0x02);
uint16_t speed=0;
uint16_t angle = 50;
uint16_t playtime =50;
ros::NodeHandle  nh;
uint16_t receive;
void servo_cb(const std_msgs::UInt16& cmd_msg) {
    receive = cmd_msg.data;
    switch (cmd_msg.data)
    {
    case 8:
        speed += 250;
        break;

    case 2:
        speed -= 250;
        break;

    case 4:
        angle += 125;
        break;
    case 6:
        angle -= 125;
        break;

    default:
        angle = 512;
        speed = 50;

        break;
    }
}

ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);
rosserial_arduino::Adc adc_msg;
ros::Publisher p("adc", &adc_msg);

void setup() {
    servoSerial1.begin(115200);
    servo1.setPosition(angle, playtime);
    servo2.setSpeed(speed);
    nh.initNode();
    nh.advertise(p);
    nh.subscribe(sub);

}
uint16_t position;
void loop() {

    servo1.setPosition(angle, playtime);
    servo2.setSpeed(speed);
    position = servo2.readPosition();
    adc_msg.adc2 = receive;
    adc_msg.adc0 = servo1.readPosition();
    adc_msg.adc1 = servo2.readPosition();
    adc_msg.adc3 = 0;
    adc_msg.adc4 = 0;
    adc_msg.adc5 = 0;
    p.publish(&adc_msg);
    nh.spinOnce();
    delay(100);
}
edit retag flag offensive close merge delete