How to get odom_trans for a robot in gazebo?

asked 2017-07-27 07:50:01 -0600

dinesh gravatar image

I am trying to write a state_publisher node which will subscribe to the joint_states from the gazebo robot and than use it to move robot in rviz. But i am not sure how to send the odom_trans to rviz and how to set its value. My prgram to send joint_state to rviz is:

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <control_msgs/JointTrajectoryControllerState.h>
#include <tf/transform_broadcaster.h>
#include <iostream>

using namespace std;

ros::Publisher joint_pub;
void
cloud_cb (const control_msgs::JointTrajectoryControllerStateConstPtr& input)
{
tf::TransformBroadcaster broadcaster;
    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "axis";

    double angle = 0; const double degree = M_PI/180;
            //update joint_state
        joint_state.header.stamp = ros::Time::now();

        joint_state.name.resize(input->joint_names.size());
        joint_state.position.resize(input->joint_names.size());
        for(size_t i=0;i<joint_state.name.size();i++) {
           joint_state.name[i] = input->joint_names[i];
           joint_state.position[i] = input->actual.positions[i];
            }

        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle)*2;
        odom_trans.transform.translation.y = sin(angle)*2;
        odom_trans.transform.translation.z = .7;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);
angle += degree/4;

    }


int
main(int argc, char** argv)
{
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n; 
    joint_pub = n.advertise<sensor_msgs::JointState>("valkyrie/joint_states", 1);
    ros::Subscriber sub = n.subscribe("/valkyrie/controller/state", 1, cloud_cb);
    ros::spin();
}

When i run this node only the robot joints moves in the rviz w.r.t each other. I want the robot to move according to he world frame also. So how to set the value of odom_trans and how to send it to rviz. right now when i search for the odom and axix frame name is rviz, i can't see them.

edit retag flag offensive close merge delete