Print linear velocity and angle to terminal [Help Wanted]

[user98765467]

I'm trying to modify the create_driver.cpp very similar to this, but I'm trying to write the linear velocity and angle to the terminal window instead. So far everything I've tried breaks the controller. Any suggestions?

  //Automatically log battery % in terminal window once per minute
  ros::Rate rate(loop_hz_minute);
  while (ros::ok())
  {
      ROS_INFO("[LOG] Battery level %.2f %%", (robot_->getBatteryCharge() / robot_->getBatteryCapacity()) * 100.0); // print to terminal
      usleep(60000000);  // HZ - 1 minute = 60000000 microseconds
  }

[jacobperron]

If you want to modify the driver, you can query the estimated velocity with robot_->getVel(). I suggest adding your print/log statements somewhere in the main update method:

https://github.com/AutonomyLab/create_autonomy/blob/90e597ea4d85cde1ec32a1d43ea2dd0b4cbf481c/ca_driver/src/create_driver.cpp#L273

But avoid blocking with a loop otherwise the driver will not function properly (ie. you should not need a while (ros::ok()) loop).

---

Alternatively, you should be able to introspect the same velocity estimate on the odometry topic with tools like rostopic echo. There shouldn't really be a need to modify the driver if you use this approach.

[user98765467]

thanks! i think i'm close to getting it working. any idea why getVel() doesnt publish anything? putting this code in the main update method still appears to break the controller.

https://imgur.com/wm39q8p

[jacobperron]

geVel() returns a struct. You should try accessing the members:

create::Vel vel = robot_->getVel();
vel.x;
vel.y;
vel.yaw;

To reiterate my previous comment, do not put a while(ros::ok()) loop inside the main update method.