tf setup for sensor on a gimbal on a robot.

asked 2016-06-19 10:56:44 -0600

uwleahcim gravatar image

updated 2016-10-24 09:04:20 -0600

ngrennan gravatar image

I looked through the tf setup, but it only show updating tf for a fixed system at arbitrary 1Hz interval.

Anyways, I plan on using sensor information to navigate a map. So my tf tree will be something like this



I have a node that grabs the gimbal and odom information from the robot platform 60 times a second and publishes the information to their respective topics. Thus should I update broadcast tf information in the same node?

(Note the code is just a simple rough outline of what I'm trying to do. My actual code will probably be setup with classes so I can have tracking variables).

int main(int argc, char **argv) 
    ros::init(argc, argv, "robot");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");

    MyRobotClass robot; //just some class for my robot interface

    nav_msgs::Odometry odometry;
    my_msg::Gimbal gimbal
    // simple gimbal message with header, yaw, roll, and pitch

    tf::TransformBoardcaster odom_broadcaster;
    tf::TransformBoardcaster gimbal_broadcaster;

    ros::Publisher gimbal_publisher;
    ros::Publisher global_position_publisher;

    gimbal_publisher = nh.advertise<my_msg::Gimbal>("gimbal", 10);
    odometry_publisher = nh.advertise<nav_msgs::Odometry>("odometry",10);

        time = ros::Time::now();

        // update and publish odometry
        odometry.header.frame_id = "/world";
        odometry.header.stamp = current_time;
        odometry.pose.pose.position.x = robot.position.x;
        odometry.pose.pose.position.y = robot.position.y;
        odometry.pose.pose.position.z = robot.position.z;
        odometry.pose.pose.orientation.w = robot.quaternion.q0;
        odometry.pose.pose.orientation.x = robot.quaternion.q1;
        odometry.pose.pose.orientation.y = robot.quaternion.q2;
        odometry.pose.pose.orientation.z = robot.quaternion.q3;
        odometry.twist.twist.angular.x = robot.velocity.wx;
        odometry.twist.twist.angular.y = robot.velocity.wy;
        odometry.twist.twist.angular.z = robot.velocity.wz;
        odometry.twist.twist.linear.x = robot.velocity.vx;
        odometry.twist.twist.linear.y = robot.velocity.vy;
        odometry.twist.twist.linear.z = robot.velocity.vz;

        // update and publish gimbal
        gimbal.header.frame_id = "/gimbal";
        gimbal.header.stamp= current_time;
        gimbal.roll = robot.gimbal.roll;
        gimbal.pitch = robot.gimbal.pitch;
        gimbal.yaw = robot.gimbal.yaw;

        // update tf of odom here????
        tf::Vector3 vectorOdom(robot.position.x, robot.position.y, robot.position.z);
        tf::Quaternion quatOdom(robot.quaternion.q1, robot.quaternion.q2, robot.quaternion.q3,                                                     robot.quaternion.q0);
        tf::Transform transformOdom(quatOdom, vectorOdom);

        odom_broadcaster.sendTransform(transformOdom, time, "/map", "/base_link")

        // update tf of gimbal here?
        tf::Vector3 vectorGimbal(0.0, 0.0, 0.0);
        tf::Quaternion quatGimbal;
        quatGimbal.setEuler(robot.gimbal.yaw, robot.gimbal.pitch, robot.gimbal.roll);
        tf::Transform transformGimbal(quatGimbal, vectorGimbal);

        gimbal_broadcaster.sendTransform(transformGimbal, time, "/base_link", "/gimbal")

    retrun 0;

Then I have a node using the camera obtain position of object in reference to the camera.

void imageCb(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& cam_info)
    geometry_msg::PoseStamped tagPose;

    //code that generates tag pose in reference to the camera in POSE_TAG_DATA

    tagPose.header.frame_id = "/camera";
    tagPose.header.stamp = msg->header.stamp;
    tagPose.header.seq = msg->header.seq;
    tagPose.pose = POSE_TAG_DATA; //pose of the tag


    // Need to flip axis from ...
edit retag flag offensive close merge delete