Ask Your Question
1

Help with tf and sensor & odometry and robot positioning

asked 2012-07-12 11:29:32 -0500

MarkyMark2012 gravatar image

updated 2014-01-28 17:13:00 -0500

ngrennan gravatar image

Hi All,

Soz if this question is a bit confused after much flicking through the tuts) - I am...

At the moment I'm simply trying to, in code (i.e. leaving out URDF for now) create my robot. It's simply has 7 Sonars cover 180 deg around the front.

To keep things simple I've tried to start off with the left most sensor and the one up at the front. The code I have is:

            sensor_msgs::Range sonar;
            sonar.radiation_type = sensor_msgs::Range::ULTRASOUND;
            sonar.field_of_view = (10.0/180.0) * 3.14;
            sonar.min_range = 0.0;
            sonar.max_range = 10.0; sonar.header.stamp = ros::Time::now();
            sonar.range = 1.3;
            sonar.child_frame_id = "base_link";
            sonar.header.frame_id = "sonar_1";
            sonar_pub.publish(sonar);

            sonar.header.frame_id = "sonar_2";
            sonar_pub.publish(sonar);             

            sonar_broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), 
                                                                                tf::Vector3(-0.15, 0.0, 0.0)),
                                                                                ros::Time::now(),
                                                                                "base_link", 
                                                                                "sonar_1"));

            sonar_broadcaster.sendTransform(tf::StampedTransform(tf::Transform( tf::Quaternion(1.57079633, 0, 0, 1), 
                                                                                tf::Vector3(0.0, 0.15, 0.0)),
                                                                                ros::Time::now(),
                                                                                "base_link", 
                                                                                "sonar_2"));

Where 1.57079633 is 90 degrees for Radians. But is rviz the both show up pointing in the same direction. My intention is that one is 15cm to the left of the bots centre and pointing at 90 degrees the other 15cm from cent to front and pointing fwd.

2) Then the other part I'm not sure about is the Odometry - how should I model that? So far I've just used the tutorial code.

            // first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = 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;

        // 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;

        // publish the message
        odom_pub.publish(odom);

3) Finally in rviz how should I be setting fixed frame and targetframe so that I effectively see the robot as a whole just moving around?

Many Thanks

Mark

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
5

answered 2012-07-12 12:05:39 -0500

Lorenz gravatar image

Your quaternions don't look quite right. I guess instead of constructing the quaternion with x, y, z, and w you want the constructor that takes euler angles. If I read the Bullet API docs correctly, you just need to leave out the 4th parameter, i.e. the 1.

For more information on quaternions, have a look at this question.

edit flag offensive delete link more

Comments

1

The tf::createQuaternion* functions are probably a better choice: http://ros.org/doc/fuerte/api/tf/html/c++/namespacetf.html . In this case, tf::createQuaternionFromRPY is probably best. In Fuerte, tf types are no longer simply typedefs of the Bullet types and the 3 element constructor is deprecated

Eric Perko gravatar image Eric Perko  ( 2012-07-12 13:19:51 -0500 )edit

Thanks - I'll have another crack at is over the weekend see if is starts to play ball

MarkyMark2012 gravatar image MarkyMark2012  ( 2012-07-12 20:55:20 -0500 )edit

Thanks Eric! +1

Lorenz gravatar image Lorenz  ( 2012-07-12 21:55:07 -0500 )edit

Thanks both - went with tf::createQuaternionFromRPY - did the job nicely

MarkyMark2012 gravatar image MarkyMark2012  ( 2012-07-16 04:03:23 -0500 )edit

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2012-07-12 11:29:32 -0500

Seen: 1,700 times

Last updated: Jul 12 '12