Why this->model->SetLinearVel doesn't work?
I am trying to give velocity to a model by using SetLinearVel for the model and apparently it doesn't seem to work.
I looked up this tutorial: http://gazebosim.org/tutorials?tut=se... and they are doing model->GetLink("white_link_0")->SetLinearVel({0, 1, 0});
. However, I am attempting to do this->model->SetLinearVel(ignition::math::Vector3d(2.0, 0.0, 0.0));
and it doesn't seem to work. I don't see my model moving. What is it that I am doing wrong?
The full code is given below:
namespace gazebo
{
Sparkle::Sparkle()
{
}
void Sparkle::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
ROS_INFO_STREAM("Load");
this->model = _parent; /*Store the pointer to the model*/
this->world = _parent->GetWorld();
this->robotNamespace = "";
this->physicsEngine = (this->world)->Physics();
/*Get the update rate from sdf*/
if (_sdf->HasElement("updateRate"))
{
this->updateRate = _sdf->GetElement("updateRate")->Get<double>();
}
if (_sdf->HasElement("robotNamespace"))
{
this->robotNamespace = _sdf->GetElement("robotNamespace")->Get<std::string>();
}
this->velTopic = this->robotNamespace + "/vel";
//Start up ros_node
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "sparkle_sim", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
rosnode_ = new ros::NodeHandle(this->robotNamespace);
this->ros_spinner_thread_ = boost::thread( boost::bind( &Sparkle::SimROSThread, this ) );
}
void Sparkle::SimROSThread()
{
ROS_INFO_STREAM("SimROSThread,");
ros::NodeHandle nodehandle;
ros::Subscriber velsub = nodehandle.subscribe(this->velTopic, 1, &Sparkle::changeVel, this);
ros::Rate loop_rate(10);
while (this->rosnode_->ok())
{
ROS_INFO_STREAM("Looping");
ros::spinOnce();
loop_rate.sleep( );
}
}
void Sparkle::changeVel(const geometry_msgs::Twist::ConstPtr& msg)
{
ROS_INFO_STREAM("ChangingVel");
double x = msg->linear.x;
ROS_INFO("Velocity to set is %f\n", x);
this->model->SetLinearVel(ignition::math::Vector3d(x, 0.0, 0.0));
}
void Sparkle::changePos(const gazebo_msgs::ModelState::ConstPtr& msg)
{
gazebo_msgs::ModelState newMsgs;
newMsgs.model_name = this->robotNamespace;
newMsgs.pose.position = msg->pose.position;
newMsgs.pose.orientation = msg->pose.orientation;
this->modelPub.publish(newMsgs);
}
GZ_REGISTER_MODEL_PLUGIN(Sparkle)
}
Apart from the fact that tutorial is using old math library, what is the difference between these two methods and why my method doesn't seem to work?