0
$\begingroup$

Rosanswers logo

Hello everyone,

I am new to ROS and this community. I am working with pioneer 3dx robot. My aim is to drive it in a square shape path and return back to its initial position. I am able to get quaternion values [x,y,z,w] from my robot. But for the purpose of turning the robot 90 degree I think I need rotation about z axis [yaw]. I have written the following code:

#include "ros/ros.h"

#include "nav_msgs/Odometry.h" 
#include <ros/console.h>
#include "geometry_msgs/Pose.h"
#include <tf/transform_datatypes.h>

double quatx;
double quaty;
double quatz;
double quatw;


void ComPoseCallback(const nav_msgs::Odometry::ConstPtr& msg);


int main(int argc, char **argv)
{
    ros::init(argc, argv, "Test_FK"); // connect to roscore
    ros::NodeHandle n;                                     // node object
    ros::Subscriber ComPose_sub = n.subscribe("/RosAria/pose", 1000, ComPoseCallback);
    ros::spin();
}


void ComPoseCallback(const nav_msgs::Odometry::ConstPtr& msg)            
{
    ROS_INFO("Seq: [%d]", msg->header.seq);
    ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z);
    ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    
   float linearposx=msg->pose.pose.position.x;
   float linearposy=msg->pose.pose.position.y;
   double quatx= msg->pose.pose.orientation.x;
   double quaty= msg->pose.pose.orientation.y;
   double quatz= msg->pose.pose.orientation.z;
   double quatw= msg->pose.pose.orientation.w;
   
    tf::Quaternion q(quatx, quaty, quatz, quatw);
    tf::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);
    
    ROS_INFO("Roll: [%f],Pitch: [%f],Yaw: [%f]",roll,pitch,yaw);
    return ;
}

I code compiles and do return some values. I was just spinning the robot in anti clockwise direction. Here are the values:

Position-> x: [0.000000], y: [-0.001000], z: [0.000000] Orientation-> x: [0.000000], y: [0.000000], z: [-0.054026], w: [0.998540] Roll: [0.000000],Pitch: [0.000000],Yaw: [-0.10810]

Position-> x: [0.000000], y: [-0.001000], z: [0.000000] Orientation-> x: [0.000000], y: [0.000000], z: [-0.055554], w: [0.998456] Roll: [0.000000],Pitch: [0.000000],Yaw: [-0.111165]

Position-> x: [0.000000], y: [-0.001000], z: [0.000000] Orientation-> x: [0.000000], y: [0.000000], z: [-0.055554], w: [0.998456] Roll: [0.000000],Pitch: [0.000000],Yaw: [-0.111165]

I don't understand whether i am getting the correct values or not. To my understanding Yaw is in radians and must increase with time [spinning at one posiiton].


Originally posted by Tomm on ROS Answers with karma: 59 on 2016-06-26

Post score: 2


Original comments

Comment by vsd on 2017-06-28:
Hi Tomm I would like to ask how were u able to achieve it using Navigation and actionlib. I am working on a similar project and am stuck and have less time, so i can't read from scratch. If you can walk me through the steps and probably share your code, it would be great help Thanks

Comment by jayess on 2017-07-31:
@vsd, you should ask a new question to get some help with your problem.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Spinning counter clockwise yaw should increase, yes, to convert check this answer.

If you want to move in a square you can easily use the Navigation & ActionLib to move your robot to e.g. (1,0), (1,-1), (0,1), (0, 0). It will then rotate automatically.


Originally posted by Humpelstilzchen with karma: 1504 on 2016-06-27

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Tomm on 2016-06-27:
Navigation & ActionLib is an easier way to do it. Thanks a lot.

Comment by vsd on 2017-06-27:
Hi Tomm I would like to ask how were u able to achieve it using Navigation and actionlib. I am working on a similar project and am stuck and have less time, so i can't read from scratch. If you can walk me through the steps and probably share your code, it would be great help Thanks

$\endgroup$

Your Answer

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