Master to sensor branch #195

gbaraban · 2018-08-15T20:05:49Z

This adds Sensor code (a base template, a ROS sensor, and velocity and position sensors that use the ROS sensor as the underlying basis).

This change is

…mewhere.

…co/aerial_autonomy into masterToSensorBranch

garimellagowtham

Reviewed 12 of 12 files at r3.
Reviewable status: all files reviewed, 19 unresolved discussions (waiting on @gbaraban and @msheckells)

include/aerial_autonomy/controller_connectors/rpyt_based_position_controller_drone_connector.h, line 29 at r3 (raw file):

      RPYTBasedPositionController &controller,
      ThrustGainEstimator &thrust_gain_estimator,
      Sensor<std::tuple<VelocityYawRate, PositionYaw>> *sensor = nullptr)

Use SensorPtrstd::tuple... sensor=nullptr (Use shared pointers; Don't use raw pointers

include/aerial_autonomy/sensors/odom_sensor.h, line 13 at r3 (raw file):

* @brief ros based velocity sensor
*/
class OdomSensor : public Sensor<std::tuple<VelocityYawRate, PositionYaw>> {

Rename to OdometrySensor

include/aerial_autonomy/sensors/odom_sensor.h, line 23 at r3 (raw file):

  OdomSensor(ROSSensorConfig config) : config_(config) {
    // sensor_quad_tf_ =
    //    conversions::protoTransformToTf(config_.sensor_transform());

remove commented code

include/aerial_autonomy/sensors/odom_sensor.h, line 24 at r3 (raw file):

    // sensor_quad_tf_ =
    //    conversions::protoTransformToTf(config_.sensor_transform());
    sensor_ = ROS_Sensor<nav_msgs::Odometry>(config);

Why do you need a separate config_? Are you using the config_ in this class. Remove config_

include/aerial_autonomy/sensors/odom_sensor.h, line 62 at r3 (raw file):

  * @brief sensor
  */
  ROS_Sensor<nav_msgs::Odometry> sensor_;

rename to ros_odom_sensor_

include/aerial_autonomy/sensors/odom_sensor.h, line 74 at r3 (raw file):

  * @brief variable to store sensor data
  */
  // Atomic<std::tuple<VelocityYawRate, PositionYaw>> sensor_data_;

Remove commented useless code

include/aerial_autonomy/sensors/ros_sensor.h, line 17 at r3 (raw file):

  * @brief Constructor
  */
  ROS_Sensor(ROSSensorConfig config) : config_(config) {

Rename to ROSSensor we do not use _ in class names.
For future reference, class names should be CamelCase, functions should camelCase, and variables should be camel_case

include/aerial_autonomy/sensors/ros_sensor.h, line 27 at r3 (raw file):

  SensorDataT getSensorData() {
    SensorDataT retVal = sensor_data_;
    return retVal;

you can do return SensorDataT(sensor_data_);

include/aerial_autonomy/sensors/ros_sensor.h, line 43 at r3 (raw file):

private:
  // Only works if SensorDataT is the proper ROS msg type

Add this to a proper block comment using doxygen format

include/aerial_autonomy/sensors/ros_sensor.h, line 44 at r3 (raw file):

private:
  // Only works if SensorDataT is the proper ROS msg type
  void callback(const typename SensorDataT::ConstPtr msg) {

I think you can also add a reference as &msg

include/aerial_autonomy/sensors/velocity_sensor.h, line 21 at r3 (raw file):

  */
  VelocitySensor(ROSSensorConfig config) : sensor_(config) {
    // sensor_ = ROS_Sensor<nav_msgs::Odometry>(config);

remove commented code

include/aerial_autonomy/sensors/velocity_sensor.h, line 22 at r3 (raw file):

  VelocitySensor(ROSSensorConfig config) : sensor_(config) {
    // sensor_ = ROS_Sensor<nav_msgs::Odometry>(config);
    config_ = config;

Why do you need a separate config_? Are you using the config_ in this class. Remove config_

include/aerial_autonomy/sensors/velocity_sensor.h, line 29 at r3 (raw file):

  Velocity getSensorData() {
    nav_msgs::Odometry msg = sensor_.getSensorData();
    Velocity vel_sensor_data(msg.twist.twist.linear.x, msg.twist.twist.linear.y,

use full names i.e velocity_sensor_data

include/aerial_autonomy/sensors/velocity_sensor.h, line 40 at r3 (raw file):

private:
  /*
  * @brief sensor

say something else other than its a sensor

include/aerial_autonomy/sensors/velocity_sensor.h, line 48 at r3 (raw file):

  ROSSensorConfig config_;
  /**
  * @brief sensor's origin in world frame

remove

include/aerial_autonomy/trackers/alvar_tracker.h, line 25 at r3 (raw file):

      std::chrono::duration<double> timeout = std::chrono::milliseconds(500))
      : BaseTracker(std::move(
            std::unique_ptr<TrackingStrategy>(new IdTrackingStrategy(4)))),

Why are we using id tracking strategy as opposed to closest one? Is this coming from master

param/matrice_config.pbtxt, line 103 at r3 (raw file):

  }

  velocity_sensor_config{

We need this only for visual servoing and pick place i think. uav system does not use these sensors.

proto/velocity_sensor_config.proto, line 13 at r3 (raw file):

  * becomes invalid
  */
  optional double timeout = 2 [default = 0.5];

The timeout should be ros sensor config. And that transform should not be here

src/controller_connectors/rpyt_based_position_controller_drone_connector.cpp, line 9 at r3 (raw file):

      return false;
    }
    sensor_data = sensor_->getSensorData();

Do you have to rotate this sensor data based on transform?

gbaraban

Reviewable status: 5 of 18 files reviewed, 19 unresolved discussions (waiting on @garimellagowtham, @gbaraban, and @msheckells)

include/aerial_autonomy/controller_connectors/rpyt_based_position_controller_drone_connector.h, line 29 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Use SensorPtrstd::tuple... sensor=nullptr (Use shared pointers; Don't use raw pointers

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 13 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Rename to OdometrySensor

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 23 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

remove commented code

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 24 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Why do you need a separate config_? Are you using the config_ in this class. Remove config_

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 62 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

rename to ros_odom_sensor_

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 74 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Remove commented useless code

Done.

include/aerial_autonomy/sensors/ros_sensor.h, line 17 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Rename to ROSSensor we do not use _ in class names.
For future reference, class names should be CamelCase, functions should camelCase, and variables should be camel_case

Done.

include/aerial_autonomy/sensors/ros_sensor.h, line 27 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

you can do return SensorDataT(sensor_data_);

Done.

include/aerial_autonomy/sensors/ros_sensor.h, line 43 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Add this to a proper block comment using doxygen format

Done.

include/aerial_autonomy/sensors/ros_sensor.h, line 44 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

I think you can also add a reference as &msg

No change, but good to know.

include/aerial_autonomy/sensors/velocity_sensor.h, line 21 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

remove commented code

Done.

include/aerial_autonomy/sensors/velocity_sensor.h, line 22 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Why do you need a separate config_? Are you using the config_ in this class. Remove config_

Done.

include/aerial_autonomy/sensors/velocity_sensor.h, line 29 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

use full names i.e velocity_sensor_data

Done.

include/aerial_autonomy/sensors/velocity_sensor.h, line 40 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

say something else other than its a sensor

Done.

include/aerial_autonomy/sensors/velocity_sensor.h, line 48 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

remove

Done.

include/aerial_autonomy/trackers/alvar_tracker.h, line 25 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Why are we using id tracking strategy as opposed to closest one? Is this coming from master

Waiting on comment from Matt, probably best to take master instead.

param/matrice_config.pbtxt, line 103 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

We need this only for visual servoing and pick place i think. uav system does not use these sensors.

Should I remove all of the sensor configs from param files??

proto/velocity_sensor_config.proto, line 13 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

The timeout should be ros sensor config. And that transform should not be here

Removing this file entirely.

src/controller_connectors/rpyt_based_position_controller_drone_connector.cpp, line 9 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Do you have to rotate this sensor data based on transform?

TransformedSensorData added to all sensors. Done.

garimellagowtham

Reviewed 14 of 14 files at r4.
Reviewable status: all files reviewed, 7 unresolved discussions (waiting on @garimellagowtham, @gbaraban, and @msheckells)

include/aerial_autonomy/sensors/odom_sensor.h, line 22 at r4 (raw file):

  */
  OdometrySensor(ROSSensorConfig config) : ros_odom_sensor_(config) {
    //    ros_odom_sensor_.reset(new ROSSensor<nav_msgs::Odometry>(config));

remove commented code

include/aerial_autonomy/sensors/odom_sensor.h, line 92 at r4 (raw file):

  */
  ROSSensor<nav_msgs::Odometry> ros_odom_sensor_;
  tf::Transform local_transform_;

Add comments

include/aerial_autonomy/sensors/pose_sensor.h, line 51 at r4 (raw file):

private:
  ROSSensor<geometry_msgs::TransformStamped> sensor_;
  tf::Transform local_transform_;

add comments

include/aerial_autonomy/sensors/velocity_sensor.h, line 23 at r4 (raw file):

  */
  VelocitySensor(ROSSensorConfig config) : sensor_(config) {
    //    sensor_.reset(ROSSensor<nav_msgs::Odometry>(config));

remove

param/matrice_config.pbtxt, line 103 at r3 (raw file):

Previously, gbaraban wrote…

Should I remove all of the sensor configs from param files??

For now, add the sensor config to the ones you will be testing.

src/sensors/pose_sensor.cpp, line 4 at r4 (raw file):

PoseSensor::PoseSensor(ROSSensorConfig config) : sensor_(config) {
  //  sensor_.reset(new ROSSensor<geometry_msgs::TransformStamped>(config));

remove

msheckells

Reviewed 3 of 12 files at r3, 11 of 14 files at r4.
Reviewable status: all files reviewed, 15 unresolved discussions (waiting on @garimellagowtham and @gbaraban)

include/aerial_autonomy/sensors/base_sensor.h, line 23 at r4 (raw file):

  virtual SensorDataT getSensorData() = 0;
  /**
  * @brief gets the latest sensor data

Comment should explain how it is transformed

include/aerial_autonomy/sensors/odom_sensor.h, line 53 at r4 (raw file):

  }
  /**
  * @brief gives sensor data

Transformed how?

include/aerial_autonomy/sensors/odom_sensor.h, line 63 at r4 (raw file):

    tf::Vector3 rate(msg->twist.twist.angular.x, msg->twist.twist.angular.y,
                     msg->twist.twist.angular.z);
    velocity = velocity * rotation_only_transform;

is this transforming from sensor to quad frame? Shouldn't this be rotation_only_transform * velocity?

include/aerial_autonomy/sensors/odom_sensor.h, line 64 at r4 (raw file):

                     msg->twist.twist.angular.z);
    velocity = velocity * rotation_only_transform;
    rate = velocity * rotation_only_transform;

should be rate = rotation_only_transform * rate

include/aerial_autonomy/sensors/odom_sensor.h, line 74 at r4 (raw file):

        msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    tf::Transform pyTransform(orientation, position);
    pyTransform = pyTransform * local_transform_;

pyTransform is sensor_in_sensor_origin and local_transform_ is sensor_in_robot frame right? If so, I don't understand what this transform is supposed to do.

include/aerial_autonomy/sensors/pose_sensor.h, line 50 at r4 (raw file):

private:
  ROSSensor<geometry_msgs::TransformStamped> sensor_;

Why have an internal ROSSensor instead of deriving from ROSSensor?

include/aerial_autonomy/trackers/alvar_tracker.h, line 25 at r3 (raw file):

Previously, gbaraban wrote…

Waiting on comment from Matt, probably best to take master instead.

Should use closest. We had it hardcoded to 4 in Gabe's project i think.

proto/ros_sensor_config.proto, line 15 at r4 (raw file):

  optional string topic = 3;
  /**
  * Transform from sensor to quad center.

to robot center. don't assume quad

src/sensors/pose_sensor.cpp, line 17 at r4 (raw file):

tf::StampedTransform PoseSensor::getTransformedSensorData() {
  tf::StampedTransform raw_data = getSensorData();
  tf::Transform transformed_data = raw_data * local_transform_;

local_transform_ * raw_data

gbaraban

Reviewable status: all files reviewed, 15 unresolved discussions (waiting on @garimellagowtham, @gbaraban, and @msheckells)

include/aerial_autonomy/sensors/base_sensor.h, line 23 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

Comment should explain how it is transformed

Done

include/aerial_autonomy/sensors/odom_sensor.h, line 22 at r4 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

remove commented code

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 53 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

Transformed how?

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 92 at r4 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Add comments

Done.

include/aerial_autonomy/sensors/pose_sensor.h, line 50 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

Why have an internal ROSSensor instead of deriving from ROSSensor?

Gowtham and I decided that wrapping a ROSSensor made more sense.

include/aerial_autonomy/sensors/pose_sensor.h, line 51 at r4 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

add comments

Done.

include/aerial_autonomy/sensors/velocity_sensor.h, line 23 at r4 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

remove

Done.

include/aerial_autonomy/trackers/alvar_tracker.h, line 25 at r3 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

Should use closest. We had it hardcoded to 4 in Gabe's project i think.

Done.

proto/ros_sensor_config.proto, line 15 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

to robot center. don't assume quad

Done.

src/sensors/pose_sensor.cpp, line 4 at r4 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

remove

Done.

gbaraban

Reviewable status: all files reviewed, 15 unresolved discussions (waiting on @garimellagowtham, @gbaraban, and @msheckells)

include/aerial_autonomy/sensors/odom_sensor.h, line 63 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

is this transforming from sensor to quad frame? Shouldn't this be rotation_only_transform * velocity?

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 64 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

should be rate = rotation_only_transform * rate

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 74 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

pyTransform is sensor_in_sensor_origin and local_transform_ is sensor_in_robot frame right? If so, I don't understand what this transform is supposed to do.

Done.

src/sensors/pose_sensor.cpp, line 17 at r4 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

local_transform_ * raw_data

No, the other way. Comments added to header to clarify why.

gbaraban

Reviewable status: all files reviewed, 15 unresolved discussions (waiting on @garimellagowtham, @gbaraban, and @msheckells)

param/matrice_config.pbtxt, line 103 at r3 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

For now, add the sensor config to the ones you will be testing.

Removed them all. We can add things back as necessary.

garimellagowtham

Reviewed 8 of 8 files at r5.
Reviewable status: all files reviewed, 9 unresolved discussions (waiting on @msheckells and @gbaraban)

include/aerial_autonomy/sensors/base_sensor.h, line 26 at r5 (raw file):

  * transform
  */
  virtual SensorDataT getTransformedSensorData() = 0;

Say that it gets the sensor data in robot frame

include/aerial_autonomy/sensors/pose_sensor.h, line 50 at r4 (raw file):

Previously, gbaraban wrote…

Gowtham and I decided that wrapping a ROSSensor made more sense.

@msheckells We cannot derive from ROSSensor since it is templated on sensor type which is geometry msgs transform stamped as opposed to tf Stamped transform

gbaraban

Reviewable status: all files reviewed, 9 unresolved discussions (waiting on @msheckells and @garimellagowtham)

include/aerial_autonomy/sensors/base_sensor.h, line 26 at r5 (raw file):

Previously, garimellagowtham (Gowtham Garimella) wrote…

Say that it gets the sensor data in robot frame

Done.

msheckells

Reviewed 1 of 1 files at r6.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on @garimellagowtham and @gbaraban)

include/aerial_autonomy/sensors/odom_sensor.h, line 65 at r6 (raw file):

    tf::Vector3 rate(msg->twist.twist.angular.x, msg->twist.twist.angular.y,
                     msg->twist.twist.angular.z);
    velocity = velocity + rate.cross(transform_translate);

apply rotation_only_transform to rate.cross term. Call the new variable quad_velocity

include/aerial_autonomy/sensors/odom_sensor.h, line 66 at r6 (raw file):

                     msg->twist.twist.angular.z);
    velocity = velocity + rate.cross(transform_translate);
    velocity = rotation_only_transform * velocity;

don't need this

include/aerial_autonomy/sensors/odom_sensor.h, line 67 at r6 (raw file):

    velocity = velocity + rate.cross(transform_translate);
    velocity = rotation_only_transform * velocity;
    rate = rotation_only_transform * rate;

call this quad_rate

include/aerial_autonomy/sensors/odom_sensor.h, line 77 at r6 (raw file):

        msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    tf::Transform pyTransform(orientation, position);
    pyTransform = pyTransform * local_transform_;

call this quad_in_sensor_origin

include/aerial_autonomy/sensors/velocity_sensor.h, line 52 at r6 (raw file):

        angular_velocity_data.cross(transform_translation) +
        linear_velocity_data;
    transformed_velocity = rotation_only_transform * transformed_velocity;

Make same changes as for odom_sensor

gbaraban

Reviewable status: 16 of 18 files reviewed, 6 unresolved discussions (waiting on @garimellagowtham and @msheckells)

include/aerial_autonomy/sensors/odom_sensor.h, line 65 at r6 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

apply rotation_only_transform to rate.cross term. Call the new variable quad_velocity

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 66 at r6 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

don't need this

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 67 at r6 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

call this quad_rate

Done.

include/aerial_autonomy/sensors/odom_sensor.h, line 77 at r6 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

call this quad_in_sensor_origin

Done.

include/aerial_autonomy/sensors/velocity_sensor.h, line 52 at r6 (raw file):

Previously, msheckells (Matt Sheckells) wrote…

Make same changes as for odom_sensor

Done.

msheckells

Reviewed 2 of 2 files at r7.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @garimellagowtham)

gbaraban and others added 5 commits August 13, 2018 23:25

VINS updates to master. Still need to add sensor offset transforms so…

00b1737

…mewhere.

Move ddp stuff to if arm plugins found

6f844b7

Merge branch 'fix_ddp_airm_dependencies' of https://github.com/jhu-as…

51e922c

…co/aerial_autonomy into masterToSensorBranch

Compiles clean

9b672d3

Formatting to satisfy clang requirements

3eca217

gbaraban requested review from garimellagowtham and msheckells August 15, 2018 20:07

gbaraban self-assigned this Aug 15, 2018

garimellagowtham requested changes Aug 15, 2018

View reviewed changes

gbaraban added 3 commits August 16, 2018 12:40

pose_sensor changed to use ros_sensor

215e9e6

Sensor Transform Added & PR modifications made (mostly)

4c1a78c

Removed velocity sensor config proto, OBE because of ROS sensor config

cd6f366

gbaraban commented Aug 17, 2018

View reviewed changes

garimellagowtham requested changes Aug 17, 2018

View reviewed changes

msheckells requested changes Aug 20, 2018

View reviewed changes

gbaraban commented Aug 27, 2018

View reviewed changes

Simple Formatting Changes

cf0e428

gbaraban commented Aug 27, 2018

View reviewed changes

Velocity transform fix and sensor config removed

420ec96

garimellagowtham requested changes Sep 11, 2018

View reviewed changes

gbaraban commented Sep 14, 2018

View reviewed changes

Slight comment change in base sensor

2ae3c89

msheckells requested changes Sep 17, 2018

View reviewed changes

Velocity and Odom Sensor changes

e150f04

gbaraban commented Sep 17, 2018

View reviewed changes

msheckells approved these changes Sep 17, 2018

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Master to sensor branch #195

Master to sensor branch #195

gbaraban commented Aug 15, 2018 •

edited by msheckells

Loading

garimellagowtham left a comment

gbaraban left a comment

garimellagowtham left a comment

msheckells left a comment

gbaraban left a comment

gbaraban left a comment

gbaraban left a comment

garimellagowtham left a comment

gbaraban left a comment

msheckells left a comment

gbaraban left a comment

msheckells left a comment

Master to sensor branch #195

Are you sure you want to change the base?

Master to sensor branch #195

Conversation

gbaraban commented Aug 15, 2018 • edited by msheckells Loading

garimellagowtham left a comment

Choose a reason for hiding this comment

gbaraban left a comment

Choose a reason for hiding this comment

garimellagowtham left a comment

Choose a reason for hiding this comment

msheckells left a comment

Choose a reason for hiding this comment

gbaraban left a comment

Choose a reason for hiding this comment

gbaraban left a comment

Choose a reason for hiding this comment

gbaraban left a comment

Choose a reason for hiding this comment

garimellagowtham left a comment

Choose a reason for hiding this comment

gbaraban left a comment

Choose a reason for hiding this comment

msheckells left a comment

Choose a reason for hiding this comment

gbaraban left a comment

Choose a reason for hiding this comment

msheckells left a comment

Choose a reason for hiding this comment

gbaraban commented Aug 15, 2018 •

edited by msheckells

Loading