Ask Your Question

Revision history [back]

Mobile Robot Teleoperation in Rviz

Hello,

I am trying to teleoperate a mobile robot model with a PS3 joystick on Rviz. My system is Ubuntu 12.04 with Groovy. I've the ps3joy package installed. I want the linear values of two analog joysticks to be my velocity references of the wheels. I wrote a joint state publisher (which is subscribed to the joystick node) and a odometry transform braodcaster. In the launch file I also run the robot state publisher. But when I launch it doesn't work. When I look at the rqt graph it seems my joint state publisher doesn't publish anything (I've also checked it with rostopic echo). Joy node publishes decently. So I've tried another code (without using a class) but that didn't fix either. Rviz doesn't see a transform between joints and base.

My launch file:

<launch>
      <arg name="gui" default="False" />
      <param name="robot_description" textfile="$(find agv)/urdf/agv3.urdf" />
      <param name="use_gui" value="$(arg gui)"/>

      <node respawn="true" pkg="joy"
        type="joy_node" name="joy_node" >
      <param name="dev" type="string" value="/dev/input/js0" />
      <param name="deadzone" value="0.12" />
      </node>

      <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
      <node name="rviz" pkg="rviz" type="rviz" args="-d $(find agv)/urdf.rviz" />
      <!-- Axes -->
      <param name="axis_linear_L" value="0" type="int"/>
      <param name="axis_linear_R" value="2" type="int"/>
      <param name="scale_linear_L" value="5" type="double"/>
      <param name="scale_linear_R" value="5" type="double"/>

      <node pkg="agv" type="jsPublisher"  name="Joint_State_Publisher" ></node>
      <node pkg="agv" type="odomTF"  name="Odometry_Tf_Broadcaster" ></node>
</launch>

Urdf of the mobile robot:

<robot name="AGV">
<link name="base_link">
    <visual>
        <geometry>
            <box size="0.49 .82 .2"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0.01"/>
        <material name="gray">
            <color rgba="0 0 0 0.6"/>
        </material>
    </visual>
</link>

<link name="wheel_left">
    <visual>
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
        <origin rpy="0 1.57079633 0" xyz="0 0 0"/>
        <material name="black">
            <color rgba="1 0 0 1"/>
        </material>
    </visual>
</link>

<link name="wheel_right">
    <visual>
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
        <origin rpy="0 1.57079633 0" xyz="0 0 0"/>
        <material name="black"/>
    </visual>
</link>

<joint name="base_to_wheel_left" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left"/>
    <origin xyz="-0.22 0 0"/>
    <axis xyz="1 0 0"/>
</joint>

<joint name="base_to_wheel_right" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_right"/>     
    <origin xyz="0.22 0 0"/>
    <axis xyz="1 0 0"/>
</joint>
</robot>

Joint state publisher:

class TeleopAgv
{
public:
  TeleopAgv();

private:
  void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
  ros::NodeHandle nh_;

  int linearL_, linearR_;
  double l_scale_L, l_scale_R;
  ros::Publisher pos_pub_;
  ros::Subscriber joy_sub_;

};

TeleopAgv::TeleopAgv():
  linearL_(1),
  linearR_(2)
{

  nh_.param("axis_linear_L", linearL_, linearL_);
  nh_.param("axis_linear_R", linearR_, linearR_);
  nh_.param("scale_linear_L", l_scale_L, l_scale_L);
  nh_.param("scale_linear_R", l_scale_R, l_scale_R);

  pos_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_state_pub", 1);
  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy",1000, &TeleopAgv::joyCallback, this);

}

void TeleopAgv::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
      sensor_msgs::JointState joint_state;
      joint_state.header.stamp = ros::Time::now();
      joint_state.name.resize(3);
      joint_state.velocity.resize(3);
      joint_state.name[0] ="base";
      joint_state.name[1] ="left_wheel";
      joint_state.name[2] ="right_wheel";
      joint_state.velocity[1] = l_scale_L*joy->axes[linearL_];
  joint_state.velocity[2] = l_scale_R*joy->axes[linearR_];
}
 int main(int argc, char** argv) { 
   ros::init(argc, argv, "jsPublisher");
   TeleopAgv teleop_agv;
   pos_pub_.publish(joint_state);
   ros::spin();
  }

Alternative joint state publisher:

float vel_L, vel_R;

void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{ 
  vel_L = joy->axes[linearL_];
  vel_R = joy->axes[linearR_];
}

int main(int argc, char** argv) {
ros::init(argc, argv, "jsPublisher");
void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
ros::NodeHandle nh_;

int linearL_, linearR_;
double l_scale_L, l_scale_R;
nh_.param("axis_linear_L", linearL_, linearL_);
nh_.param("axis_linear_R", linearR_, linearR_);
    nh_.param("scale_linear_L", l_scale_L, l_scale_L);
nh_.param("scale_linear_R", l_scale_R, l_scale_R);

ros::Rate loop_rate(30);

ros::Publisher pos_pub_;
ros::Subscriber joy_sub_;

sensor_msgs::JointState joint_state;
joint_state.header.stamp = ros::Time::now();
    joint_state.name.resize(3);
    joint_state.velocity.resize(3);
    joint_state.name[0] ="base";
    joint_state.name[1] ="left_wheel";
    joint_state.name[2] ="right_wheel";

pos_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_state_pub", 1);
joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy",1000, joyCallback);

while(ros::ok())
{

    joint_state.velocity[1] = l_scale_L*vel_L;
        joint_state.velocity[2] = l_scale_R*vel_R;
    pos_pub_.publish(joint_state);
    loop_rate.sleep();
}
return 0;    }

Odometry transform broadcaster:

float angle;
float jsVelR_,jsVelL_;
const int length = 0.8;

void jsCallback(const sensor_msgs::JointState jointState)
{
    jsVelL_ = jointState.velocity[1];
    jsVelR_ = jointState.velocity[2];   
    angle = (jsVelR_-jsVelL_)/length;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "odom_transform");
    ros::NodeHandle nh_;
    tf::TransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";
    ros::Subscriber js_sub_;
     js_sub_ = nh_.subscribe<sensor_msgs::JointState>("joint_state_pub", 100, jsCallback);
    ros::Rate loop_rate(30);

    while (ros::ok()) {

        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle);
        odom_trans.transform.translation.y = sin(angle);
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);
        broadcaster.sendTransform(odom_trans);
        loop_rate.sleep();
    }
    return 0;
}

rqt graph:

image description

This is what I see on rviz:

image description

Mobile Robot Teleoperation in Rviz

Hello,

I am trying to teleoperate a mobile robot model with a PS3 joystick on Rviz. My system is Ubuntu 12.04 with Groovy. I've the ps3joy package installed. I want the linear values of two analog joysticks to be my velocity references of the wheels. I wrote a joint state publisher (which is subscribed to the joystick node) and a an odometry transform braodcaster. In the launch file I also run the robot state publisher. But when I launch it doesn't work. When I look at the rqt graph try to move the model, I have to choose /odom frame and robot only turns around a circle with /odom frame in center, it seems can't move linearly.

Also, I'm not very sure about what should I send to odometry tf broadcaster. I took the joint_state.velocity[] as linear velocity of the wheel. I calculate the change in the robot's angle by substituting the previous angular velocity from the present angular velocity as;

jsVelL_ = jointState.velocity[1]; //left wheel velocity reference from left joystick
jsVelR_ = jointState.velocity[2];   // right wheel velocity reference from right joystick
angleDelta = ((jsVelR_-jsVelL_)/length)-angle; //angular velocity - prev. ang. velocity
angle = angleDelta;

So I'm passing this to odometry transform broadcaster as;

odom_trans.transform.translation.x = cos(angle);
odom_trans.transform.translation.y = sin(angle);
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);
broadcaster.sendTransform(odom_trans);

So my joint state publisher doesn't publish anything (I've also checked it questions;

  1. Why can I only move around the /odom frame in circle?
  2. Shouldn't /odom frame drift with rostopic echo). Joy node publishes decently. So the robot and thus shouldn't I find and select a higher order world frame as fixed?
  3. I treated joint_state.velocity[] as linear velocities of my wheels. Is that true or are they angular velocities?
  4. Is my usage of odom tf broadcaster right as I've tried another code (without using a class) but that didn't fix either. Rviz doesn't see a transform between joints and base. to use the change in the robot's angle? If not how should it be?

Thank you.

My launch file:

<launch>
      <arg name="gui" default="False" />
      <param name="robot_description" textfile="$(find agv)/urdf/agv3.urdf" />
      <param name="use_gui" value="$(arg gui)"/>

      <node respawn="true" pkg="joy"
        type="joy_node" name="joy_node" >
      <param name="dev" type="string" value="/dev/input/js0" />
      <param name="deadzone" value="0.12" />
      </node>

      <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
      <node name="rviz" pkg="rviz" type="rviz" args="-d $(find agv)/urdf.rviz" />
      <!-- Axes -->
      <param name="axis_linear_L" value="0" type="int"/>
      <param name="axis_linear_R" value="2" type="int"/>
      <param name="scale_linear_L" value="5" type="double"/>
      <param name="scale_linear_R" value="5" type="double"/>

      <node pkg="agv" type="jsPublisher"  name="Joint_State_Publisher" ></node>
      <node pkg="agv" type="odomTF"  name="Odometry_Tf_Broadcaster" ></node>
</launch>

Urdf of the mobile robot:

<robot name="AGV">
<link name="base_link">
    <visual>
        <geometry>
            <box size="0.49 .82 .2"/>
        </geometry>
        <origin rpy="0 0 0" xyz="0 0 0.01"/>
        <material name="gray">
            <color rgba="0 0 0 0.6"/>
        </material>
    </visual>
</link>

<link name="wheel_left">
    <visual>
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
        <origin rpy="0 1.57079633 0" xyz="0 0 0"/>
        <material name="black">
            <color rgba="1 0 0 1"/>
        </material>
    </visual>
</link>

<link name="wheel_right">
    <visual>
        <geometry>
            <cylinder length="0.05" radius="0.1"/>
        </geometry>
        <origin rpy="0 1.57079633 0" xyz="0 0 0"/>
        <material name="black"/>
    </visual>
</link>

<joint name="base_to_wheel_left" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_left"/>
    <origin xyz="-0.22 0 0"/>
    <axis xyz="1 0 0"/>
</joint>

<joint name="base_to_wheel_right" type="continuous">
    <parent link="base_link"/>
    <child link="wheel_right"/>     
    <origin xyz="0.22 0 0"/>
    <axis xyz="1 0 0"/>
</joint>
</robot>

Joint state publisher:

class TeleopAgv
{
public:
  TeleopAgv();
  ~TeleopAgv();

private:
  void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
  ros::NodeHandle nh_;

  int linearL_, linearR_;
  double l_scale_L, l_scale_R;
  ros::Publisher pos_pub_;
vel_pub_;
  ros::Subscriber joy_sub_;

};

TeleopAgv::TeleopAgv():
  linearL_(1),
  linearR_(2)
{
   ROS_INFO("Calling the constructor of class TeleopAgv");
  nh_.param("axis_linear_L", linearL_, linearL_);
  nh_.param("axis_linear_R", linearR_, linearR_);
  nh_.param("scale_linear_L", l_scale_L, l_scale_L);
  nh_.param("scale_linear_R", l_scale_R, l_scale_R);

  pos_pub_ vel_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_state_pub", 1);
  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy",1000, &TeleopAgv::joyCallback, this);

}

TeleopAgv::~TeleopAgv()
{
    ROS_INFO("Calling the destructor of class TeleopAgv");
}

void TeleopAgv::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
      sensor_msgs::JointState joint_state;
      joint_state.header.stamp = ros::Time::now();
      joint_state.name.resize(3);
      joint_state.velocity.resize(3);
      joint_state.name[0] ="base";
      joint_state.name[1] ="left_wheel";
      joint_state.name[2] ="right_wheel";
      joint_state.velocity[1] = l_scale_L*joy->axes[linearL_];
  joint_state.velocity[2] = l_scale_R*joy->axes[linearR_];
}
 int main(int argc, char** argv) { 
   ros::init(argc, argv, "jsPublisher");
   TeleopAgv teleop_agv;
   pos_pub_.publish(joint_state);
   ros::spin();
  }

Alternative joint state publisher:

float vel_L, vel_R;

void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{ 
  vel_L = joy->axes[linearL_];
  vel_R = joy->axes[linearR_];
  vel_pub_.publish(joint_state);
}

int main(int argc, char** argv) {
 ros::init(argc, argv, "jsPublisher");
void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
ros::NodeHandle nh_;

int linearL_, linearR_;
double l_scale_L, l_scale_R;
nh_.param("axis_linear_L", linearL_, linearL_);
nh_.param("axis_linear_R", linearR_, linearR_);
    nh_.param("scale_linear_L", l_scale_L, l_scale_L);
nh_.param("scale_linear_R", l_scale_R, l_scale_R);

ros::Rate loop_rate(30);

ros::Publisher pos_pub_;
ros::Subscriber joy_sub_;

sensor_msgs::JointState joint_state;
joint_state.header.stamp = ros::Time::now();
    joint_state.name.resize(3);
    joint_state.velocity.resize(3);
    joint_state.name[0] ="base";
    joint_state.name[1] ="left_wheel";
    joint_state.name[2] ="right_wheel";

pos_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_state_pub", 1);
joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy",1000, joyCallback);

while(ros::ok())
{

    joint_state.velocity[1] = l_scale_L*vel_L;
        joint_state.velocity[2] = l_scale_R*vel_R;
    pos_pub_.publish(joint_state);
    loop_rate.sleep();
}
return 0;        TeleopAgv teleop_agv;
    ros::spin();
}

Odometry transform broadcaster:

float angle;
float jsVelR_,jsVelL_;
int angle=0;
int angleDelta,jsVelR_,jsVelL_;
const int length = 0.8;

void jsCallback(const sensor_msgs::JointState jointState)
{
    jsVelL_ = jointState.velocity[1];
    jsVelR_ = jointState.velocity[2];   
    angleDelta = ((jsVelR_-jsVelL_)/length)-angle; 
    angle = (jsVelR_-jsVelL_)/length;
angleDelta;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "odom_transform");
    ros::NodeHandle nh_;
    tf::TransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";
     ros::Subscriber js_sub_;
      js_sub_ = nh_.subscribe<sensor_msgs::JointState>("joint_state_pub", 100, jsCallback);
    ros::Rate loop_rate(30);

     while (ros::ok()) {

        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle);
        odom_trans.transform.translation.y = sin(angle);
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);
        broadcaster.sendTransform(odom_trans);
        loop_rate.sleep();
ros::spinOnce();
    }
    return 0;
0; }

rqt graph:

image description

This is what I see on rviz:

image description