Skip to content

Commit beff16e

Browse files
edkochedkoch
authored
Merge pull request #46 from AutoModality/BB-424/collision_avoidance
feat: cleaned up code to prep for speed calculations
2 parents 5d6a53e + 7e42347 commit beff16e

File tree

3 files changed

+53
-26
lines changed

3 files changed

+53
-26
lines changed

‎include/vb_util_lib/kinematics.h‎

Lines changed: 36 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -74,13 +74,14 @@ class Kinematics {
7474
setPose(h.frame_id, h.stamp, p, debug);
7575
}
7676
void setOdometry(const nav_msgs::Odometry::ConstPtr& msg, int debug=-1) {
77-
// setPose(msg->child_frame_id, msg->header.stamp, msg->pose.pose, debug);
7877
setPose(msg->child_frame_id, msg->header.stamp, msg->pose.pose, debug);
7978
setLinearVelocity(msg->twist.twist.linear);
79+
setAngularVelocity(msg->twist.twist.angular);
8080
}
8181
void setOdometry(nav_msgs::Odometry& msg, int debug=-1) {
8282
setPose(msg.child_frame_id, msg.header.stamp, msg.pose.pose, debug);
8383
setLinearVelocity(msg.twist.twist.linear);
84+
setAngularVelocity(msg.twist.twist.angular);
8485
}
8586

8687
void getPose(geometry_msgs::Pose& p);
@@ -91,10 +92,15 @@ class Kinematics {
9192
/*
9293
* The following setters set the core attributes without regard to the frame of reference.
9394
* Therefore it assumes that the coordinates are in the same frame as this object.
95+
* Does not update the velocity.
9496
*/
95-
void setPosition(const geometry_msgs::Point& p, int debug=-1);
96-
void setPosition(const AM::Point& p, int debug=-1);
97-
void setPosition(double x, double y, double z, int debug=-1);
97+
void setPosition(const ros::Time& update_time, const geometry_msgs::Point& p, int debug=-1) {
98+
setPosition(update_time, p.x, p.y, p.z, debug);
99+
}
100+
void setPosition(const ros::Time& update_time, const AM::Point& p, int debug=-1) {
101+
setPosition(update_time, p.x, p.y, p.z, debug);
102+
}
103+
void setPosition(const ros::Time& update_time, double x, double y, double z, int debug=-1);
98104
void checkPosition(double x, double y, double z, int debug=-1);
99105
void setAltitude(double a) { position_.z = a; }
100106
const AM::Point& position() const {
@@ -131,6 +137,13 @@ class Kinematics {
131137
return velLinear_;
132138
}
133139

140+
/**
141+
* Get the linear velocity with respect to some frame of reference
142+
*/
143+
const Eigen::Vector3d& getLinearVelocity(std::string frame) const {
144+
return velLinearFrame_;
145+
}
146+
134147
bool isPoseInitialized() const {
135148
return update_cnt_ > 0;
136149
}
@@ -216,6 +229,10 @@ class Kinematics {
216229
return update_stale_;
217230
}
218231

232+
void setSpeedFilter(double speedFilter) {
233+
speed_filter_ = speedFilter;
234+
}
235+
219236
protected:
220237
ros::NodeHandle *nh_;
221238

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

260+
/**
261+
* Used to update the velocity given a new position
262+
*/
263+
void updateVelocity(double pos_x, double pos_y, double pos_z, double delta_t, int debug=-1);
264+
243265
/**
244266
* Identifier for the object represented by these kinematics
245267
*/
@@ -265,6 +287,16 @@ class Kinematics {
265287
*/
266288
Eigen::Vector3d velLinear_;
267289

290+
/**
291+
* Cached linear velocity in a certain frame of reference
292+
*/
293+
Eigen::Vector3d velLinearFrame_;
294+
295+
/**
296+
* Speed filter constant
297+
*/
298+
double speed_filter_ {1.0};
299+
268300
/**
269301
* Angular velocity
270302
*/

‎src/vb_util_lib/kinematics.cpp‎

Lines changed: 16 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ void Kinematics::setPose(const std::string& source_frame, const ros::Time& updat
9898

9999
if (source_frame == frame_id_)
100100
{
101-
setPosition(p.position, debug);
101+
setPosition(update_time, p.position, debug);
102102
setAttitude(p.orientation);
103103
ROS_DEBUG_STREAM_COND(debug_ && publisher_, "XXXXXXXX SAME FRAME = \n" << p);
104104
}
@@ -115,7 +115,7 @@ void Kinematics::setPose(const std::string& source_frame, const ros::Time& updat
115115
cur_pt.header.stamp = update_time;
116116
// transform point
117117
tf2::doTransform(cur_pt, ts_pt, ts);
118-
setPosition(ts_pt.pose.position, debug);
118+
setPosition(update_time, ts_pt.pose.position, debug);
119119
setAttitude(ts_pt.pose.orientation);
120120
ROS_DEBUG_STREAM_COND(debug_ && publisher_, "XXXXXXXX FROM POINT = \n" << cur_pt);
121121
ROS_DEBUG_STREAM_COND(debug_ && publisher_, "XXXXXXXX TRANSFORM = \n" << ts);
@@ -164,23 +164,15 @@ bool Kinematics::getPoseStamped(std::string& target_frame_id, geometry_msgs::Po
164164
return true;
165165
}
166166

167-
void Kinematics::setPosition(const geometry_msgs::Point& p, int debug) {
168-
checkPosition(p.x, p.y, p.z, debug);
169-
position_.x = p.x;
170-
position_.y = p.y;
171-
position_.z = p.z;
172-
}
173-
174-
void Kinematics::setPosition(const AM::Point& p, int debug) {
175-
checkPosition(p.x, p.y, p.z, debug);
176-
position_ = p;
177-
}
178-
179-
void Kinematics::setPosition(double x, double y, double z, int debug) {
167+
void Kinematics::setPosition(const ros::Time& update_time, double x, double y, double z, int debug) {
180168
checkPosition(x, y, z, debug);
181169
position_.x = x;
182170
position_.y = y;
183171
position_.z = z;
172+
173+
//SSS Calculate speed
174+
175+
update_time_ = update_time;
184176
}
185177

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

221+
void Kinematics::updateVelocity(double x, double y, double z, double t,
222+
int debug) {
223+
}
224+
229225
void Kinematics::setVelocity(const geometry_msgs::Vector3& linear,
230226
const geometry_msgs::Vector3& angular) {
231227
setLinearVelocity(linear);
@@ -435,12 +431,11 @@ void Kinematics::sendKinematics() {
435431
void Kinematics::updateCB(const nav_msgs::Odometry::ConstPtr& msg) {
436432
// LOG_MSG(topic_+"-R", msg, 2); // TODO (dan) bag_logger
437433
setUpdated(true);
438-
// setPose(msg->child_frame_id, msg->header.stamp, msg->pose.pose);
439-
setPosition(msg->pose.pose.position);
434+
setPosition(msg->header.stamp, msg->pose.pose.position);
440435
setAttitude(msg->pose.pose.orientation);
441436
setLinearVelocity(msg->twist.twist.linear);
442-
// update_time_ = msg->header.stamp;
443-
update_time_ = ros::Time::now();
437+
update_time_ = msg->header.stamp;
438+
// update_time_ = ros::Time::now();
444439

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

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

512507
printf("Kinematics: setPosition - position\n");
513-
setPosition(p.pose.position);
508+
setPosition(ros::Time::now(), p.pose.position);
514509
print("TEST");
515510

516511
printf("Kinematics: setPose\n");

‎src/vb_util_lib/object_state.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ void ObjectState::calculateTargetENU() {
104104
}
105105

106106
// update the target ENU state
107-
target_current_ENU.setPosition(Xt, Yt, target_current_RFU.position().z);
107+
target_current_ENU.setPosition(target_current_RFU.getUpdateTime(), Xt, Yt, target_current_RFU.position().z);
108108
target_current_ENU.setAttitude(target_current_RFU.getAttitude());
109109
// targetCurrentENU.publishTransform();
110110
// 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);

0 commit comments

Comments
 (0)