# Giving a 180 degree rotation to a robot This post is a wiki. Anyone with karma >75 is welcome to improve it.

Hi All,

I am trying to build a robot which will stop 2 units length ahead of the obstacle and will have a 180 degree (pi radian) rotation. After the rotation it will again start moving to other obstacles. For that I am writing a Gazebo Plugin with the following OnUpdate() function

public : void OnUpdate() {

double FinalVelocity = 1.0;
double minRange;
double ReducingFactor = 10.0;
double AngularPosition;
double xPositionCurr, yPositionCurr, zPositionCurr;
double xPositionFin, yPositionFin, zPositionFin;

std::vector<double> ranges;
minRange = this->laser->GetRangeMax();

this->laser->GetRanges(ranges);
for(std::vector<double>::const_iterator it = ranges.begin(); it != ranges.end(); ++it) {

if(*it < minRange){
minRange = *it;
}
}
if(minRange <= 1.0){
FinalVelocity = 0.0;
this->model->SetLinearVel(math::Vector3(FinalVelocity, 0, 0));
}
else {
double ErrorMargin  = minRange - 1.0;
FinalVelocity = ErrorMargin / ReducingFactor;
this->model->SetLinearVel(math::Vector3(FinalVelocity, 0, 0));
//FinalVelocity = std::max(FinalVelocity, 0.05)
}

//      this->model->SetLinearVel(math::Vector3(FinalVelocity, 0, 0));
if(FinalVelocity == 0.0){
this->left_wheel_joint_->SetForce(0, 0.9);
this->right_wheel_joint_->SetForce(0,-0.9);
}
}


The model rotates continuously. How can I ensure that after 180 degree rotation the model stops and moves forward? Thanks for your kind suggestions.

Thanks and Regards, Debjit.

edit retag close merge delete

It looks like the minRange value is never set. Is that correct?

I previously posted a partial code. The complete code is here:

I previously posted a partial code. The complete code is here:

Sort by » oldest newest most voted This post is a wiki. Anyone with karma >75 is welcome to improve it.

Dear my Friend, You can this work in two ways, in first way you should count number of iterations that robot needs to reaching your target degree with 0.9 speed and then define a counter in your code to checking it. it's a simple way but solve your problem. in second way you can work with z parameter in orientation of robot, and get current z of orientation and then add 180 degree to it and when your robot reaches to your target angle then send zero speed to wheels. i do this in this way, but i don't do it in a plugin. just i should say the z parameter changed between 0 to -1 when head of robot turn to right, and 0 to 1 when head of robot turned to left, 0 to 1 is equal 0 to 180. you should subscribe to "~/pose/info" topic.

subInfo  = nodeInfo->Subscribe("~/pose/info", &Manage::parseInfo, this);

void Manage::parseInfo(ConstPosesStampedPtr &_msg)
{
if ( MGDebug )
qDebug()<<"<<<MG>>> In ParseInfo Function.";

QStringList listmsg;
QString     msg(_msg->ShortDebugString().c_str()), tempMsg, rmmsg;

// Keep A Copy of Message For Next Parse
tempMsg = msg;

///***/// Time Extraction

// Cut Message Before Sec
rmmsg = "sec:";
msg   = msg.remove(0,msg.indexOf(rmmsg));

// Cut After nsec
rmmsg = "nsec";
msg   = msg.remove(msg.indexOf(rmmsg),msg.size());

// Remove sec:
rmmsg = "sec: ";
msg   = msg.remove(rmmsg);

// Convert Timefrom String to Int
time = msg.toInt();

if ( MGDebug )
qDebug()<<"<<<MG>>> Parsed Time of Simulator: "<<time;

///***///

///***/// Extract Position of Robot
msg = tempMsg;

// Cut Message Before First Position
rmmsg = "position";
msg   = msg.remove(0,msg.indexOf(rmmsg));

// Cut After First Oriention
rmmsg = "orientation";
msg   = msg.remove(msg.indexOf(rmmsg),msg.size());

msg   = msg.remove("position { x: ");
msg   = msg.remove("y: ");
msg   = msg.remove("z: ");
msg   = msg.remove(" } ");

// Split Msg to Seperated String
listmsg.clear();
listmsg = msg.split(QRegExp("\\s+"));

// Set Position to Its Varibales
posRobot.x = QString(listmsg).toDouble();
posRobot.y = QString(listmsg).toDouble();
posRobot.z = QString(listmsg).toDouble();

if ( MGDebug )
qDebug()<<"<<<MG>>> Parsed Position of Robot, X: "<<posRobot.x<<" , Y: "<<posRobot.y<<" , Z: "<<posRobot.z;

///***////

///***/// Extarct Orientation of Robot
msg = tempMsg;

// Cut Message Before First Orientation
rmmsg = "orientation";
msg   = msg.remove(0,msg.indexOf(rmmsg));

// Cut After Second Pose
rmmsg = "pose";
msg   = msg.remove(msg.indexOf(rmmsg,2),msg.size());

msg   = msg.remove("orientation { x: ");
msg   = msg.remove("y: ");
msg   = msg.remove("z: ");
msg   = msg.remove("w: ");
msg   = msg.remove(" } } ");

// Split Msg to Seperated String
listmsg.clear();
listmsg = msg.split(QRegExp("\\s+"));

// Set Position to Its Varibales
rotRobot.x = QString(listmsg).toDouble();
rotRobot.y = QString(listmsg).toDouble();
rotRobot.z = QString(listmsg).toDouble();
rotRobot.w = QString(listmsg).toDouble();

if ( rotRobot.w < 0 )
rotRobot.z = rotRobot.z * -1;

if ( MGDebug )

///***///
}

more

Dear Vahid Azizi, How can I get the z parameter? Are you suggesting the z parameter from the model->GetWorldPose().pos function? If not please suggest.. Thanks for your suggestion..

Dear my friend, you should subscribe to ("/pose/info") topic and then parse message and extract the z parameter in orientation, I've added my code in main answer, in final rotRobot.z is head angle of robot. if you need more help please say. This post is a wiki. Anyone with karma >75 is welcome to improve it.

One issue is that you are calling model->SetLinearVel. This works by applying an impulse to the "Canonical link" of the model that causes that link to have the desired velocity. Currently, the "Canonical link" is the first link listed in the model (see comments in Model.cc).

If your model has wheels but you set the velocity of the main robot body, the friction forces may react strongly to the impulses applied by SetLinearVell. For the example of a wheeled robot, I would recommend using SetLinearVel or SetForce on the wheels of the robot rather than the robot body.

more

@ scpeters, so basically you are suggesting something like this->leftwheeljoint_->SetForce(0, FinalVelocity) instead of the this->model->SelLinearVel(math::Vector3(FinalVelocity,0,0)) ? I did the following:

this->leftwheeljoint->SetForce(0, FinalVelocity); this->rightwheeljoint->SetForce(0,FinalVelocity);

But even after getting a minRange = 0, the robo dont stop.. Possibly I am doing something wrong.. Looking for your suggestions.

If you're going to do joint->SetForce(), then you need to make the force act in opposition to the current joint velocity. You can also do `joint->SetVelocity(0, FinalVelocity)'.