Skip to content

Commit e8e2666

Browse files
cassinajcassinaj
committed
Removed osr depend.
1 parent 25bc930 commit e8e2666

File tree

6 files changed

+50
-81
lines changed

6 files changed

+50
-81
lines changed

‎CMakeLists.txt‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,6 @@ find_package(catkin REQUIRED
3535
image_transport
3636
fl
3737
dbot
38-
osr
3938
dbot_ros
4039
)
4140

@@ -102,7 +101,6 @@ catkin_package(
102101
robot_state_pub
103102
image_transport
104103
fl
105-
osr
106104
dbot
107105
dbot_ros
108106
DEPENDS

‎package.xml‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,6 @@
3232
<build_depend>robot_state_pub</build_depend>
3333
<build_depend>image_transport</build_depend>
3434
<build_depend>fl</build_depend>
35-
<build_depend>osr</build_depend>
3635
<build_depend>dbot</build_depend>
3736
<build_depend>dbot_ros</build_depend>
3837

@@ -51,7 +50,6 @@
5150
<run_depend>robot_state_pub</run_depend>
5251
<run_depend>image_transport</run_depend>
5352
<run_depend>fl</run_depend>
54-
<run_depend>osr</run_depend>
5553
<run_depend>dbot</run_depend>
5654
<run_depend>dbot_ros</run_depend>
5755

‎source/dbrt/kinematics_from_urdf.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@
4040
// tools
4141
#include <dbrt/part_mesh_model.hpp>
4242

43-
#include <osr/pose_vector.hpp>
43+
#include <dbot/pose/pose_vector.hpp>
4444

4545
class KinematicsFromURDF
4646
{

‎source/dbrt/robot_state.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@
1919
#include <memory>
2020
#include <mutex>
2121

22-
#include <osr/euler_vector.hpp>
23-
#include <osr/rigid_bodies_state.hpp>
22+
#include <dbot/pose/euler_vector.hpp>
23+
#include <dbot/pose/rigid_bodies_state.hpp>
2424

2525
// TODO: THERE IS A PROBLEM HERE BECAUSE WE SHOULD NOT DEPEND ON THIS FILE,
2626
// SINCE IT IS IN A PACKAGE WHICH IS BELOW THIS PACKAGE.

‎source/dbrt/tracker/fusion_tracker.cpp‎

Lines changed: 44 additions & 71 deletions
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,9 @@
1717
* \author Jan Issac (jan.issac@gmail.com)
1818
*/
1919

20+
#include <dbrt/tracker/fusion_tracker.h>
2021
#include <ros/ros.h>
2122
#include <sensor_msgs/JointState.h>
22-
#include <dbrt/tracker/fusion_tracker.h>
2323

2424
#include <dbot_ros/util/ros_interface.hpp>
2525

@@ -51,7 +51,6 @@ void FusionTracker::initialize(const std::vector<State>& initial_states)
5151

5252
void FusionTracker::run_rotary_tracker()
5353
{
54-
5554
while (running_)
5655
{
5756
usleep(10);
@@ -72,7 +71,6 @@ void FusionTracker::run_rotary_tracker()
7271
current_angle_measurement = current_angle_measurement_;
7372
}
7473

75-
7674
std::lock_guard<std::mutex> belief_buffer_lock(
7775
joints_obsrv_belief_buffer_mutex_);
7876
for (auto joints_obsrv_entry : joints_obsrvs_buffer_local)
@@ -88,16 +86,17 @@ void FusionTracker::run_rotary_tracker()
8886
joints_belief_entry.joints_obsrv_entry.obsrv);
8987
current_time = joints_belief_entry.joints_obsrv_entry.timestamp;
9088
current_angle_measurement =
91-
joints_belief_entry.joints_obsrv_entry.obsrv;
89+
joints_belief_entry.joints_obsrv_entry.obsrv;
9290

93-
joints_belief_entry.beliefs =
94-
gaussian_joint_tracker_->beliefs();
91+
joints_belief_entry.beliefs = gaussian_joint_tracker_->beliefs();
9592

9693
// update sliding window of belief and joints obsrv entries
9794
joints_obsrv_belief_buffer_.push_back(joints_belief_entry);
9895
if (joints_obsrv_belief_buffer_.size() > 10000)
9996
{
100-
PInfo("Belief buffer max size reached ... popping");
97+
ROS_WARN(
98+
"Belief buffer max size reached ... discarding oldest "
99+
"belief. It seems the visual tracker is too slow.");
101100
joints_obsrv_belief_buffer_.pop_front();
102101
}
103102
}
@@ -113,9 +112,7 @@ void FusionTracker::run_rotary_tracker()
113112

114113
void FusionTracker::run_visual_tracker()
115114
{
116-
117-
std::shared_ptr<VisualTracker> particle_tracker =
118-
visual_tracker_factory_();
115+
std::shared_ptr<VisualTracker> particle_tracker = visual_tracker_factory_();
119116

120117
State current_state;
121118
double garbage;
@@ -188,8 +185,8 @@ void FusionTracker::run_visual_tracker()
188185
// #4
189186
auto transition = std::static_pointer_cast<
190187
fl::LinearTransition<VisualTracker::State,
191-
VisualTracker::Noise,
192-
VisualTracker::Input>>(
188+
VisualTracker::Noise,
189+
VisualTracker::Input>>(
193190
particle_tracker->filter()->transition());
194191

195192
// #5
@@ -206,7 +203,6 @@ void FusionTracker::run_visual_tracker()
206203
ros_image_updated_ = false;
207204
}
208205

209-
210206
auto image = ri::to_eigen_vector<double>(
211207
ros_image, camera_data_->downsampling_factor());
212208
State current_state;
@@ -245,31 +241,8 @@ void FusionTracker::run_visual_tracker()
245241
joints_obsrv_belief_buffer_local.pop_back();
246242
}
247243

248-
// timestamp check
249-
double maxdelta = 0;
250-
double avdelta = 0;
251-
JointsObsrvEntry prev;
252-
prev.timestamp = 0;
253-
for (auto& entry: joints_obsrvs_buffer_)
254-
{
255-
if (entry.timestamp < prev.timestamp)
256-
{
257-
PV(entry.timestamp - prev.timestamp);
258-
PInfo("constructed queue is wrong");
259-
}
260-
261-
if (prev.timestamp > 0)
262-
{
263-
double delta = entry.timestamp - prev.timestamp;
264-
maxdelta = std::max(maxdelta, delta);
265-
avdelta += delta;
266-
}
267-
268-
prev = entry;
269-
}
270244
MEASURE("total time for visual processing");
271245
}
272-
273246
}
274247

275248
int FusionTracker::find_belief_entry(const std::deque<JointsBeliefEntry>& queue,
@@ -292,26 +265,24 @@ int FusionTracker::find_belief_entry(const std::deque<JointsBeliefEntry>& queue,
292265
index++;
293266
}
294267

295-
if (i_t > j_t)
296-
{
297-
std::cout << ">>>>>>>>> ";
298-
}
299-
else
300-
{
301-
302-
std::cout << "<<<<<<<<< ";
303-
304-
}
305-
std::cout << "Waiting for processed joint observation beliefs. "
306-
<< "Unprocessed observations: " << joints_obsrvs_buffer_.size()
307-
<< " ("
308-
<< " image t: " << i_t << ", joints t: " << j_t << "; diff: "
309-
<< i_t - j_t
310-
<< ")"
311-
<< std::endl;
268+
// debugging information
269+
// if (i_t > j_t)
270+
// {
271+
// std::cout << ">>>>>>>>> ";
272+
// }
273+
// else
274+
// {
275+
// std::cout << "<<<<<<<<< ";
276+
// }
277+
// std::cout << "Waiting for processed joint observation beliefs. "
278+
// << "Unprocessed observations: " << joints_obsrvs_buffer_.size()
279+
// << " ("
280+
// << " image t: " << i_t << ", joints t: " << j_t
281+
// << "; diff: " << i_t - j_t << ")" << std::endl;
312282
// std::cout.precision(20);
313283
// std::cout << "could not find a matching belief for time stamp "
314-
// << timestamp << " (delta: " << timestamp - max << ")" << std::endl;
284+
// << timestamp << " (delta: " << timestamp - max << ")" <<
285+
// std::endl;
315286
// std::cout << "latest processed obsrv: " << max << std::endl;
316287
// std::cout << "oldest processed obsrv: " << min << std::endl;
317288
// std::cout << "unprocessed obsrvs size: " << joints_obsrvs_buffer_.size()
@@ -404,7 +375,8 @@ void FusionTracker::current_state_and_time(State& current_state,
404375
}
405376

406377
void FusionTracker::current_things(State& current_state,
407-
double& current_time, JointsObsrv& current_angle_measurement) const
378+
double& current_time,
379+
JointsObsrv& current_angle_measurement) const
408380
{
409381
std::lock_guard<std::mutex> state_lock(current_state_mutex_);
410382

@@ -425,14 +397,13 @@ void FusionTracker::joints_obsrv_callback(
425397

426398
if (joints_obsrvs_buffer_.size() > 1000000)
427399
{
428-
ROS_WARN("Obsrv buffer max size reached.");
400+
ROS_WARN_STREAM("Obsrv buffer max size (" << 1000000 << ") reached.");
429401
joints_obsrvs_buffer_.pop_front();
430402
}
431403

432-
433-
if(j_t > entry.timestamp)
404+
if (j_t > entry.timestamp)
434405
{
435-
ROS_ERROR_STREAM("joint measurements not ordered!!!");
406+
ROS_ERROR_STREAM("Joint measurements not ordered!");
436407
}
437408

438409
j_t = entry.timestamp;
@@ -444,28 +415,30 @@ void FusionTracker::image_obsrv_callback(const sensor_msgs::Image& ros_image)
444415

445416
ros_image_updated_ = true;
446417
ros_image_ = ros_image;
447-
ros_image_.header.stamp.fromSec(
448-
ros_image_.header.stamp.toSec() - camera_delay_);
418+
ros_image_.header.stamp.fromSec(ros_image_.header.stamp.toSec() -
419+
camera_delay_);
449420

450421
std::lock_guard<std::mutex> lock_joint_obsrv(joints_obsrv_buffer_mutex_);
451422

452-
if(i_t > ros_image_.header.stamp.toSec())
423+
if (i_t > ros_image_.header.stamp.toSec())
453424
{
454-
ROS_ERROR_STREAM("image measurements not ordered!!!");
425+
ROS_ERROR_STREAM("Image measurements not ordered!");
455426
}
456427

457428
i_t = ros_image_.header.stamp.toSec();
458429

459-
if(i_t > j_t)
430+
if (i_t > j_t)
460431
{
461432
ROS_ERROR_STREAM("latest image newer than latest joint angles!!!"
462-
<< std::endl <<
463-
"angle stamp: " << j_t << " imaga stamp : " << i_t
464-
<< std::endl <<
465-
"difference: " << i_t - j_t << std::endl);
466-
433+
<< std::endl
434+
<< "angle stamp: "
435+
<< j_t
436+
<< " imaga stamp : "
437+
<< i_t
438+
<< std::endl
439+
<< "difference: "
440+
<< i_t - j_t
441+
<< std::endl);
467442
}
468-
469443
}
470-
471444
}

‎source/dbrt/tracker/robot_tracker.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -30,9 +30,9 @@
3030
#include <dbot/camera_data.hpp>
3131
#include <dbot/object_model.hpp>
3232

33-
#include <osr/pose_vector.hpp>
34-
#include <osr/composed_vector.hpp>
35-
#include <osr/free_floating_rigid_bodies_state.hpp>
33+
#include <dbot/pose/pose_vector.hpp>
34+
#include <dbot/pose/composed_vector.hpp>
35+
#include <dbot/pose/free_floating_rigid_bodies_state.hpp>
3636

3737
#include <ros/ros.h>
3838
#include <image_transport/image_transport.h>

0 commit comments

Comments
 (0)