hbrobotics / ros_arduino_bridge

ROS + Arduino = Robot
355 stars 351 forks source link

how to subscribe encoder value in ros node #34

Closed roboticsai closed 7 years ago

roboticsai commented 7 years ago

How can i subscribe the encoder value in ros node and than use it? I tried below code but its no use:

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <iostream>
#include <ros_arduino_msgs/Digital.h>
using namespace ros_arduino_msgs;
using namespace message_filters;

void callback(const DigitalConstPtr& e1, const DigitalConstPtr& e2)
{ 
  std::cout<<"callback"<<std::endl;
  std::cout<<e1<<std::endl;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");
  std::cout<<"main"<<std::endl;
  ros::NodeHandle nh;
  message_filters::Subscriber<Digital> e1(nh, "/arduino/sensor/left_encoder_A", 10);
  message_filters::Subscriber<Digital> e2(nh, "/arduino/sensor/left_encoder_B", 10);
  TimeSynchronizer<Digital, Digital> sync(e1, e2, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

My arduino ros yaml file is:

# For a direct USB cable connection, the port name is typically
# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
# For a wireless connection like XBee, the port is typically
# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.

port: /dev/ttyACM0
baud: 57600
timeout: 0.1

rate: 50
sensorstate_rate: 10

use_base_controller: False
base_controller_rate: 10

base_frame: base_link

sensors: {
  right_encoder_A:    {pin: 18, type: Digital, rate: 5, direction: output},
  right_encoder_B:   {pin: 19, type: Digital, rate: 5, direction: output}
}

# Joint name and configuration is an example only
joints: {
    joint1: {pin: 3, init_position: 0, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False},
    joint2: {pin: 5, init_position: 0, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False},
    joint3: {pin: 6, init_position: 0, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False}
}
Fibird commented 7 years ago

@roboticsai You can not subscribe encoders' value in ros node, because no node publishes these data on some topic like "/arduino/sensor/left_encoder_#". The way you can get encoders' value is to fetch data from serial. You can call method get_encoder_counts in file "arduino_driver.py" in which it sends command to serial and returns data fetching from serial.