ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Python Publisher and C++ Subscriber problem

asked 2013-06-26 07:40:55 -0500

roman27 gravatar image

updated 2014-01-28 17:17:03 -0500

ngrennan gravatar image

hello, we cannot get any information, when we try to connect our C++ listener to the existing, working Python Publisher. Codes are given below.

Publisher

import roslib; roslib.load_manifest('razor_imu_9dof')
import rospy

import serial
import string
import math

from time import time, sleep
from sensor_msgs.msg import Imu
from razor_imu_9dof.msg import RazorImu


import tf

grad2rad = 3.141592/180.0

rospy.init_node("node")
pub = rospy.Publisher('imu', Imu)
pubRaw = rospy.Publisher('imuRaw', RazorImu)
#print pubRaw

#epub = rospy.Publisher('imuRaw', Imu)

imuMsg = Imu()
imuRawMsg = RazorImu()
imuMsg.orientation_covariance = [999999 , 0 , 0,
0, 9999999, 0,
0, 0, 999999]
imuMsg.angular_velocity_covariance = [9999, 0 , 0,
0 , 99999, 0,
0 , 0 , 0.02]
imuMsg.linear_acceleration_covariance = [0.2 , 0 , 0,
0 , 0.2, 0,
0 , 0 , 0.2]

default_port='/dev/ttyUSB0'
port = rospy.get_param('device', default_port)
# Check your COM port and baud rate
ser = serial.Serial(port=port,baudrate=57600, timeout=1)

#f = open("Serial"+str(time())+".txt", 'w')

roll=0
pitch=0
yaw=0
rospy.sleep(5) # Sleep for 8 seconds to wait for the board to boot then only write command.
ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text 
while 1:
    line = ser.readline()
    line = line.replace("#YPR=","")
    line = line.replace("#YPRAMG=","")   # Delete "#YPR="
    #f.write(line)                     # Write to the output log file
    words = string.split(line,",")    # Fields split
    #wort5 = float(words[5]) 
    #print wort3, wort4, wort5

    if len(words) > 2:
        try:
            yaw = float(words[0])*grad2rad
            pitch = -float(words[1])*grad2rad
            roll = -float(words[2])*grad2rad

            # Publish message
            imuMsg.linear_acceleration.x = float(words[3]) # tripe axis accelerator meter
            imuMsg.linear_acceleration.y = float(words[4])
            imuMsg.linear_acceleration.z = float(words[5])
        #print imuMsg.linear_acceleration.x, imuMsg.linear_acceleration.y ,imuMsg.linear_acceleration.z

        #epub.publish(imuMsg)

            imuMsg.angular_velocity.x = float(words[9]) #gyro
            imuMsg.angular_velocity.y = float(words[10]) 
            imuMsg.angular_velocity.z = float(words[11])
        except Exception as e:
            print e

        q = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
        imuMsg.orientation.x = q[0] #magnetometer
        imuMsg.orientation.y = q[1]
        imuMsg.orientation.z = q[2]
        imuMsg.orientation.w = q[3]
    #time.sleep( 2 )
    #print imuMsg.orientation.x, imuMsg.orientation.y, imuMsg.orientation.z, imuMsg.orientation.w
        imuMsg.header.stamp= rospy.Time.now()
        imuMsg.header.frame_id = 'base_link'
        pub.publish(imuMsg)
    print imuMsg #Gibt alle Sensorwerte in einer Art Tabelle aus


        # Publish Raw message from Razor board
        imuRawMsg.yaw = yaw
        imuRawMsg.pitch = pitch
        imuRawMsg.roll = roll
        pubRaw.publish(imuRawMsg)
    #print imuRawMsg


ser.close
#f.close

Subscriber

#include "ros/ros.h"
#include "std_msgs/String.h"


void chatterIMU(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());

}


int main(int argc, char **argv)
{

  ros::init(argc, argv, "listener");

  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("imu", 1000, chatterIMU);

  ros::spin();

  return 0;
}

Could you please say, what we do wrong. According to this tutorial http://www.ros.org/wiki/ROSNodeTutorialC%2B%2BPython subscriber/publisher does not depend on the source language, as we understood. Thank you.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2013-06-26 09:45:52 -0500

thebyohazard gravatar image

You are right: it doesn't depend on the source language.

First of all, you're publishing an imu message, but you're subscribing to a std_msgs::String. It'll probably fuss at you for that.

Once you fix the message types to be the same, I'd suggest using the rostopic command line tool to echo what your publisher is publishing and publish dummy messages to your subscriber. That way you can test and debug the nodes independently.

edit flag offensive delete link more

Question Tools

Stats

Asked: 2013-06-26 07:40:55 -0500

Seen: 5,974 times

Last updated: Jun 26 '13