Ask Your Question
0

Subscribe to quaternion

asked 2013-08-01 05:40:42 -0500

boog gravatar image

updated 2013-08-01 07:35:20 -0500

Hi I am trying to subscribe to a quaternion being published by an nxt compass and using it for odometry orientation. So far the code doesn't work and I am wondering what I've done wrong. Thanks.

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <nxt_msgs/Compass.h>
#include <boost/assign/list_of.hpp>
#include <math.h>
#include "std_msgs/String.h"

void chatterCallback(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, "odometry_publisher");

  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;


  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();



  ros::Subscriber sub = n.subscribe<nxt_msgs::Compass>("compass", 1000, chatterCallback);



  ros::Rate r(30.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages
    current_time = ros::Time::now();

    //compute odometry in a typical way given the velocities of the robot
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double compass_yaw = atan (compass_angle);

    double compass_angle = 90;
    x += delta_x;
    y += delta_y;

    //std::cout << "x=" << x << " y=" << y << std::endl;
    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = sub;

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = last_time;//current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;

    odom.pose.covariance =  boost::assign::list_of(1e-3) (0)   (0)  (0)  (0)  (0)
      (0) (1e-3)  (0)  (0)  (0)  (0)
      (0)   (0)  (1e6) (0)  (0)  (0)
      (0)   (0)   (0) (1e6) (0)  (0)
      (0)   (0)   (0)  (0) (1e6) (0)
      (0)   (0)   (0)  (0)  (0)  (1e3) ; 

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    odom.twist.covariance =  boost::assign::list_of(1e-3) (0)   (0)  (0)  (0)  (0)
      (0) (1e-3)  (0)  (0)  (0)  (0)
      (0)   (0)  (1e6) (0)  (0)  (0)
      (0)   (0)   (0) (1e6) (0)  (0)
      (0)   (0)   (0)  (0) (1e6) (0)
      (0)   (0)   (0)  (0)  (0)  (1e3) ; 

    //publish the message
    odom_pub.publish(odom);
    //std::cout << "published: [" << last_time << " " << current_time << " " << dt << "]" << std::endl;
    last_time = current_time;
    r.sleep();
  }
}

Compile error:

In function ‘int main(int, char**)’:
/opt/ros/groovy/share/fakeOdometry/src/main.cpp:60:43: error: conversion from ‘ros::Subscriber’ to non-scalar type ‘geometry_msgs::Quaternion’ requested
make[2]: *** [CMakeFiles/fake_odometry.dir/src/main.cpp ...
(more)
edit retag flag offensive close merge delete

Comments

1

So is it a compile time error or a run time error? Could you post the output of the compiler in the former case and say what the program does do in the latter?

tbh gravatar imagetbh ( 2013-08-01 06:28:00 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2013-08-01 08:49:49 -0500

tbh gravatar image

A subscriber is an object and isn't the same as the messages it's subscribed to. So the line geometry_msgs::Quaternion odom_quat = sub; is definitely a problem.

I'd probably put most of your code into the callback. Then, whenever you get a message, it can calculate the odometry message and publish it.

Also, your subscriber template doesn't match the callback parameter type. You'll want to make them match.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-08-01 05:40:42 -0500

Seen: 866 times

Last updated: Aug 01 '13