0
$\begingroup$

Rosanswers logo

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 an odometry transform braodcaster. In the launch file I also run the robot state publisher. When I try to move the model, I have to choose /odom frame and robot only turns around a circle with /odom frame in center, it 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 questions;

  1. Why can I only move around the /odom frame in circle?
  2. Shouldn't /odom frame drift with 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 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 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);

  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_];
  vel_pub_.publish(joint_state);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "jsPublisher");
    TeleopAgv teleop_agv;
    ros::spin();
}

Odometry transform broadcaster:

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 = 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);
        ros::spinOnce();
    }
    return 0; }

Originally posted by emreay on ROS Answers with karma: 90 on 2014-12-19

Post score: 0

Improve this question
$\endgroup$

2 Answers 2

Reset to default
0
$\begingroup$

Rosanswers logo

In both yours:

while (ros::ok()) {
...
}

you must call:

ros::spinOnce();

otherwise callbacks are not called/executed

Furthermore in the first script you call the constructor of the class, but af the end it will automatically call your destructor since you intialize your class, do nothing, and then you go back to main.

To understand what I mean try something like that:

TeleopAgv::TeleopAgv()
{
   ...
   ROS_INFO("Calling the constructor of class bla bla");
}

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

Originally posted by Andromeda with karma: 893 on 2014-12-19

This answer was NOT ACCEPTED on the original site

Post score: 1

Original comments

Comment by emreay on 2014-12-19:
Thank you. This solved my publishing problem. I have revised my question due to other problems.

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

I have found the answers for most of my questions and now I'm able to do teleoperation. I am writing the answers in case someone else might the same problems.

So the answers;

  1. In the odometry transform broadcaster, my mistake was to only create transforms between /odom and /base_link. This was the reason why I the robot only moved around a circle of odom frame. I have fixed this.

  2. I still don't know why I only see /odom frame and no higher world frame.

  3. Joint_state.velocity[] acts as what we treated. It's just a variable. So in my case since the joints are continuous they should be taken as angular velocities.

  4. I didn't have a time differential so my usage was wrong. It is fixed by;

     while (ros::ok())
 {
 ros::spinOnce(); //check new messages
 current_time = ros::Time::now(); //set current time

 nh_.getParam("wLeft",wLeft); //get left wheel angular velocity value from parameter server
 nh_.getParam("wRight",wRight); //get right wheel angular velocity value from parameter server

 vLeft = wheelRadius*wLeft; //linear velocity of left wheel
 vRight = wheelRadius*wRight; //linear velocity of rigth wheel
 vx = (vRight+vLeft)/2; //linear velocity in ROBOT frame
 wth = (vRight-vLeft)/length; //angular velocity of the ROBOT
 dt = (current_time - prev_time).toSec(); //time differential
 delta_x = vx*cos(th)*dt; //change in x axis in world frame
 delta_y = vx*sin(th)*dt; //change in y axis in world frame
 delta_th = wth*dt; //change in the orientation

 x += delta_x;
 y += delta_y;
 th += delta_th;
     //broadcast transforms
     broadcastTF(&broadcaster,"base_link",x,y,0,th);
     broadcastTF(&broadcaster,"wheel_left",x,y,0,th);
     broadcastTF(&broadcaster,"wheel_right",x,y,0,th);
     //set previous time
 prev_time = current_time;
     //rest until loop rate is done
     loop_rate.sleep();}//end of while

Originally posted by emreay with karma: 90 on 2014-12-25

This answer was ACCEPTED on the original site

Post score: 0

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.