Skip to content

feat: cleaned up code to prep for speed calculations #46

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Aug 12, 2020
Merged

feat: cleaned up code to prep for speed calculations #46

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 36 additions & 4 deletions include/vb_util_lib/kinematics.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,13 +74,14 @@ class Kinematics {
setPose(h.frame_id, h.stamp, p, debug);
}
void setOdometry(const nav_msgs::Odometry::ConstPtr& msg, int debug=-1) {
// setPose(msg->child_frame_id, msg->header.stamp, msg->pose.pose, debug);
setPose(msg->child_frame_id, msg->header.stamp, msg->pose.pose, debug);
setLinearVelocity(msg->twist.twist.linear);
setAngularVelocity(msg->twist.twist.angular);
}
void setOdometry(nav_msgs::Odometry& msg, int debug=-1) {
setPose(msg.child_frame_id, msg.header.stamp, msg.pose.pose, debug);
setLinearVelocity(msg.twist.twist.linear);
setAngularVelocity(msg.twist.twist.angular);
}

void getPose(geometry_msgs::Pose& p);
Expand All @@ -91,10 +92,15 @@ class Kinematics {
/*
* The following setters set the core attributes without regard to the frame of reference.
* Therefore it assumes that the coordinates are in the same frame as this object.
* Does not update the velocity.
*/
void setPosition(const geometry_msgs::Point& p, int debug=-1);
void setPosition(const AM::Point& p, int debug=-1);
void setPosition(double x, double y, double z, int debug=-1);
void setPosition(const ros::Time& update_time, const geometry_msgs::Point& p, int debug=-1) {
setPosition(update_time, p.x, p.y, p.z, debug);
}
void setPosition(const ros::Time& update_time, const AM::Point& p, int debug=-1) {
setPosition(update_time, p.x, p.y, p.z, debug);
}
void setPosition(const ros::Time& update_time, double x, double y, double z, int debug=-1);
void checkPosition(double x, double y, double z, int debug=-1);
void setAltitude(double a) { position_.z = a; }
const AM::Point& position() const {
Expand Down Expand Up @@ -131,6 +137,13 @@ class Kinematics {
return velLinear_;
}

/**
* Get the linear velocity with respect to some frame of reference
*/
const Eigen::Vector3d& getLinearVelocity(std::string frame) const {
return velLinearFrame_;
}

bool isPoseInitialized() const {
return update_cnt_ > 0;
}
Expand Down Expand Up @@ -216,6 +229,10 @@ class Kinematics {
return update_stale_;
}

void setSpeedFilter(double speedFilter) {
speed_filter_ = speedFilter;
}

protected:
ros::NodeHandle *nh_;

Expand All @@ -240,6 +257,11 @@ class Kinematics {
// of this kinematics object.
void setPose(const std::string& source_frame, const ros::Time& update_time, const geometry_msgs::Pose& p, int debug=-1);

/**
* Used to update the velocity given a new position
*/
void updateVelocity(double pos_x, double pos_y, double pos_z, double delta_t, int debug=-1);

/**
* Identifier for the object represented by these kinematics
*/
Expand All @@ -265,6 +287,16 @@ class Kinematics {
*/
Eigen::Vector3d velLinear_;

/**
* Cached linear velocity in a certain frame of reference
*/
Eigen::Vector3d velLinearFrame_;

/**
* Speed filter constant
*/
double speed_filter_ {1.0};

/**
* Angular velocity
*/
Expand Down
37 changes: 16 additions & 21 deletions src/vb_util_lib/kinematics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ void Kinematics::setPose(const std::string& source_frame, const ros::Time& updat

if (source_frame == frame_id_)
{
setPosition(p.position, debug);
setPosition(update_time, p.position, debug);
setAttitude(p.orientation);
ROS_DEBUG_STREAM_COND(debug_ && publisher_, "XXXXXXXX SAME FRAME = \n" << p);
}
Expand All @@ -115,7 +115,7 @@ void Kinematics::setPose(const std::string& source_frame, const ros::Time& updat
cur_pt.header.stamp = update_time;
// transform point
tf2::doTransform(cur_pt, ts_pt, ts);
setPosition(ts_pt.pose.position, debug);
setPosition(update_time, ts_pt.pose.position, debug);
setAttitude(ts_pt.pose.orientation);
ROS_DEBUG_STREAM_COND(debug_ && publisher_, "XXXXXXXX FROM POINT = \n" << cur_pt);
ROS_DEBUG_STREAM_COND(debug_ && publisher_, "XXXXXXXX TRANSFORM = \n" << ts);
Expand Down Expand Up @@ -164,23 +164,15 @@ bool Kinematics::getPoseStamped(std::string& target_frame_id, geometry_msgs::Po
return true;
}

void Kinematics::setPosition(const geometry_msgs::Point& p, int debug) {
checkPosition(p.x, p.y, p.z, debug);
position_.x = p.x;
position_.y = p.y;
position_.z = p.z;
}

void Kinematics::setPosition(const AM::Point& p, int debug) {
checkPosition(p.x, p.y, p.z, debug);
position_ = p;
}

void Kinematics::setPosition(double x, double y, double z, int debug) {
void Kinematics::setPosition(const ros::Time& update_time, double x, double y, double z, int debug) {
checkPosition(x, y, z, debug);
position_.x = x;
position_.y = y;
position_.z = z;

//SSS Calculate speed

update_time_ = update_time;
}

#define MAX_POS_ERR 1.0
Expand Down Expand Up @@ -226,6 +218,10 @@ void Kinematics::setOrientation(double w, double x, double y, double z) {
attitude_.throttle = 0;
}

void Kinematics::updateVelocity(double x, double y, double z, double t,
int debug) {
}

void Kinematics::setVelocity(const geometry_msgs::Vector3& linear,
const geometry_msgs::Vector3& angular) {
setLinearVelocity(linear);
Expand Down Expand Up @@ -435,12 +431,11 @@ void Kinematics::sendKinematics() {
void Kinematics::updateCB(const nav_msgs::Odometry::ConstPtr& msg) {
// LOG_MSG(topic_+"-R", msg, 2); // TODO (dan) bag_logger
setUpdated(true);
// setPose(msg->child_frame_id, msg->header.stamp, msg->pose.pose);
setPosition(msg->pose.pose.position);
setPosition(msg->header.stamp, msg->pose.pose.position);
setAttitude(msg->pose.pose.orientation);
setLinearVelocity(msg->twist.twist.linear);
// update_time_ = msg->header.stamp;
update_time_ = ros::Time::now();
update_time_ = msg->header.stamp;
// update_time_ = ros::Time::now();

// setOdometry(msg);
//print(">>>>>>> RECEIVING KINEMATICS: ", true);
Expand Down Expand Up @@ -506,11 +501,11 @@ void Kinematics::test() {
printf(" Linear velocity x:%f, y:%f, z:%f\n", van.x, van.y, van.z);

printf("Kinematics: setPosition - x,y,z\n");
setPosition(p.pose.position.x, p.pose.position.y, p.pose.position.z);
setPosition(ros::Time::now(), p.pose.position.x, p.pose.position.y, p.pose.position.z);
print("TEST");

printf("Kinematics: setPosition - position\n");
setPosition(p.pose.position);
setPosition(ros::Time::now(), p.pose.position);
print("TEST");

printf("Kinematics: setPose\n");
Expand Down
2 changes: 1 addition & 1 deletion src/vb_util_lib/object_state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ void ObjectState::calculateTargetENU() {
}

// update the target ENU state
target_current_ENU.setPosition(Xt, Yt, target_current_RFU.position().z);
target_current_ENU.setPosition(target_current_RFU.getUpdateTime(), Xt, Yt, target_current_RFU.position().z);
target_current_ENU.setAttitude(target_current_RFU.getAttitude());
// targetCurrentENU.publishTransform();
// printf("Xc:%f, Yc:%f, Dc:%f, Xt:%f, Yt:%f, YAW:%f, Ay:%f, Ac:%f, At:%f\n", Xc, Yc, Dc, Xt, Yt, vehicle_current_ENU.attitude.yaw, Ay, Ac, At);
Expand Down