Skip to content

Commit

Permalink
Changed pos_downloader.cpp and pos_uploader.cpp:
Browse files Browse the repository at this point in the history
- pos_downloader.cpp: changed the type of marker put on detected car from CUBE to SPHERE
- pos_uploader.cpp: make pos_uploader node to subscribe to obj_car/obj_label and obj_person/obj_label
  • Loading branch information
anhnv3991 committed Sep 14, 2015
1 parent eb29edf commit a52983e
Show file tree
Hide file tree
Showing 4 changed files with 52 additions and 31 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ catkin_package(
# LIBRARIES dpm
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
CATKIN_DEPENDS runtime_manager
)

###########
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>dpm</build_depend>
<run_depend>runtime_manager</run_depend>
<export>
</export>
</package>
Original file line number Diff line number Diff line change
Expand Up @@ -181,29 +181,18 @@ static void publish_car(int id, int is_current, ros::Time now,

} else {
marker.id = create_markerid(pose, 1);
marker.type = visualization_msgs::Marker::CUBE;
marker.type = visualization_msgs::Marker::SPHERE;
marker.lifetime = ros::Duration(life_time);
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
marker.color.a = alpha_percent(diffmsec);
marker.scale.x = 4.4; // #A
marker.scale.y = 1.6;
marker.scale.z = 1.0; // #1
marker.scale.x = 2.0;
marker.scale.y = 2.0;
marker.scale.z = 2.0;
marker.pose.position.z += 0.5; // == #1/2
pub.publish(marker);
dbg_out_marker(marker);

marker.id = 1 + create_markerid(pose, 1);
marker.scale.x = 3.0; // #B
marker.scale.y = 1.6;
marker.scale.z = 0.6; // #2
marker.pose = pose;
marker.pose.position.x += (4.4 - 3.0) / 2; // == (#A - #B)/2
marker.pose.position.z += 1.0 + 0.3; // == #1 + #2/2

pub.publish(marker);
dbg_out_marker(marker);
}
}

Expand Down
62 changes: 46 additions & 16 deletions ros/src/data/packages/pos_db/nodes/pos_uploader/pos_uploader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,17 @@
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>


#include <pos_db.h>
#include <cv_tracker/obj_label.h>

#define MYNAME "pos_uploader"
#define OWN_TOPIC_NAME "current_pose"
#define CAR_TOPIC_NAME "obj_car/obj_label"
#define PERSON_TOPIC_NAME "obj_person/obj_label"
#define CAR_TOPIC_NAME "obj_car/obj_pose"
#define PERSON_TOPIC_NAME "obj_person/obj_pose"

using namespace std;

Expand Down Expand Up @@ -249,24 +252,51 @@ static void* intervalCall(void *unused)
return nullptr;
}

static void car_locate_cb(const cv_tracker::obj_label& obj_label_msg)

static void car_locate_cb(const visualization_msgs::MarkerArray& obj_pose_msg)
{
if(obj_label_msg.reprojected_pos.size() > 0) {
pthread_mutex_lock(&pose_lock_);
car_num += obj_label_msg.reprojected_pos.size();
car_positions_array.push_back(obj_label_msg);
pthread_mutex_unlock(&pose_lock_);
}
if (obj_pose_msg.markers.size() > 0) {
geometry_msgs::Point tmpPoint;
cv_tracker::obj_label tmpLabel;

pthread_mutex_lock(&pose_lock_);

for (visualization_msgs::Marker tmpMarker : obj_pose_msg.markers) {
tmpPoint.x = tmpMarker.pose.position.x;
tmpPoint.y = tmpMarker.pose.position.y;
tmpPoint.z = tmpMarker.pose.position.z;

tmpLabel.reprojected_pos.push_back(tmpPoint);
}

car_positions_array.push_back(tmpLabel);
car_num += obj_pose_msg.markers.size();

pthread_mutex_unlock(&pose_lock_);
}
}

static void person_locate_cb(const cv_tracker::obj_label& obj_label_msg)
static void person_locate_cb(const visualization_msgs::MarkerArray &obj_pose_msg)
{
if(obj_label_msg.reprojected_pos.size() > 0) {
pthread_mutex_lock(&pose_lock_);
person_num += obj_label_msg.reprojected_pos.size();
person_positions_array.push_back(obj_label_msg);
pthread_mutex_unlock(&pose_lock_);
}
if (obj_pose_msg.markers.size() > 0) {
geometry_msgs::Point tmpPoint;
cv_tracker::obj_label tmpLabel;

pthread_mutex_lock(&pose_lock_);

for (visualization_msgs::Marker tmpMarker : obj_pose_msg.markers) {
tmpPoint.x = tmpMarker.pose.position.x;
tmpPoint.y = tmpMarker.pose.position.y;
tmpPoint.z = tmpMarker.pose.position.z;

tmpLabel.reprojected_pos.push_back(tmpPoint);
}

person_positions_array.push_back(tmpLabel);
person_num += obj_pose_msg.markers.size();

pthread_mutex_unlock(&pose_lock_);
}
}

static void current_pose_cb(const geometry_msgs::PoseStamped &pose)
Expand Down

0 comments on commit a52983e

Please sign in to comment.