Skip to content
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.

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

ed_msgs seperated #3

Merged
merged 4 commits into from
Oct 18, 2017
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
46 changes: 12 additions & 34 deletions .travis.yml
Original file line number Diff line number Diff line change
@@ -1,39 +1,17 @@
sudo: required
dist: trusty
language: generic
env:
- ROS_DISTRO=indigo
- ROS_DISTRO=jade
notifications:
email:
on_success: change # [always|never|change], default=change
on_failure: change # [always|never|change], default=always
sudo: true

before_install:
- CI_SOURCE_PATH=$(pwd)
- REPOSITORY_NAME=${PWD##*/}
- echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"

install:
- source <(wget -O- https://raw.githubusercontent.com/tue-robotics/tue-env/master/installer/scripts/bootstrap-ros-$ROS_DISTRO)
- tue-get install ros-$REPOSITORY_NAME
- source ~/.tue/setup.bash # source all target setup files
language: cpp

before_script:
# remove the tue-get version of our repo with the travis one
- cd "$TUE_ENV_DIR/repos/https_/github.com/tue-robotics"
- mv "$REPOSITORY_NAME.git" "$REPOSITORY_NAME.old"
- ln -s "$CI_SOURCE_PATH" "$REPOSITORY_NAME.git"
services:
- docker

# install travis' package dependencies
- cd "$TUE_SYSTEM_DIR/src"
- rm *
- tue-get update ros-$REPOSITORY_NAME
- ls -l
before_install:
- wget https://raw.githubusercontent.com/tue-robotics/tue-env/master/ci/install-package.sh
- wget https://raw.githubusercontent.com/tue-robotics/tue-env/master/ci/build-package.sh
- export PACKAGE=${TRAVIS_REPO_SLUG#*/}

# go to the catkin workspace
- cd "$TUE_SYSTEM_DIR"
install:
- bash install-package.sh --package=$PACKAGE --branch=$TRAVIS_BRANCH --commit=$TRAVIS_COMMIT --pullrequest=$TRAVIS_PULL_REQUEST

script:
- catkin build
- catkin run_tests
script:
- bash build-package.sh --package=$PACKAGE
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
project(head_ref_ed_client)

find_package(catkin REQUIRED COMPONENTS
ed
ed_msgs
head_ref
roscpp
geolib2
Expand Down
4 changes: 2 additions & 2 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,12 +10,12 @@

<buildtool_depend>catkin</buildtool_depend>

<build_depend>ed</build_depend>
<build_depend>ed_msgs</build_depend>
<build_depend>head_ref</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>geolib2</build_depend>

<run_depend>ed</run_depend>
<run_depend>ed_msgs</run_depend>
<run_depend>head_ref</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>geolib2</run_depend>
Expand Down
24 changes: 12 additions & 12 deletions src/head_ref_ed_client.cpp
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#include <actionlib/client/action_client.h>
#include "head_ref/HeadReferenceAction.h"

#include <ed/SimpleQuery.h>
#include <ed_msgs/SimpleQuery.h>

#include <geolib/datatypes.h>
#include <geolib/ros/tf_conversions.h>
Expand All @@ -12,7 +12,7 @@

struct Comp
{
bool operator()(const ed::EntityInfo& a, const ed::EntityInfo& b)
bool operator()(const ed_msgs::EntityInfo& a, const ed_msgs::EntityInfo& b)
{
geo::Vector3 a_cp, b_cp, a_diff, b_diff;
geo::convert(a.pose.position, a_cp);
Expand Down Expand Up @@ -63,15 +63,15 @@ bool inFrontOf(const geo::Vector3& p, const geo::Pose3D& pose)

// Keep track of last seen
boost::circular_buffer<std::pair<std::string, double> > cb(10);
bool getTargetEntityBasedOnLastSeen(const std::vector<ed::EntityInfo>& entities, ed::EntityInfo& e_target)
bool getTargetEntityBasedOnLastSeen(const std::vector<ed_msgs::EntityInfo>& entities, ed_msgs::EntityInfo& e_target)
{
double now = ros::Time::now().toSec();
double last_seen = now;

for (std::vector<ed::EntityInfo>::const_iterator it = entities.begin(); it != entities.end(); ++it)
for (std::vector<ed_msgs::EntityInfo>::const_iterator it = entities.begin(); it != entities.end(); ++it)
{
// Closest
const ed::EntityInfo& e = *it;
const ed_msgs::EntityInfo& e = *it;

// Loop over the circular buffer and try to find the element
bool found = false;
Expand Down Expand Up @@ -110,7 +110,7 @@ int main(int argc, char** argv){
ros::NodeHandle nh("~");
ros::NodeHandle gnh;

ros::ServiceClient ed_client = gnh.serviceClient<ed::SimpleQuery>("ed/simple_query");
ros::ServiceClient ed_client = gnh.serviceClient<ed_msgs::SimpleQuery>("ed/simple_query");
actionlib::ActionClient<head_ref::HeadReferenceAction> ac("head_ref/action_server");
tf_listener = new tf::TransformListener(nh);

Expand All @@ -127,8 +127,8 @@ int main(int argc, char** argv){
if (getRobotPoseMap(ros::Time(0), robot_pose))
{
//! Request and response
ed::SimpleQueryRequest req;
ed::SimpleQueryResponse resp;
ed_msgs::SimpleQueryRequest req;
ed_msgs::SimpleQueryResponse resp;

//! Fill the request
geo::convert(robot_pose.getOrigin(), req.center_point);
Expand All @@ -142,12 +142,12 @@ int main(int argc, char** argv){
continue;
}

std::vector<ed::EntityInfo> entities;
std::vector<ed_msgs::EntityInfo> entities;

// Get all valid entities
for (std::vector<ed::EntityInfo>::const_iterator it = resp.entities.begin(); it != resp.entities.end(); ++it)
for (std::vector<ed_msgs::EntityInfo>::const_iterator it = resp.entities.begin(); it != resp.entities.end(); ++it)
{
const ed::EntityInfo& e = *it;
const ed_msgs::EntityInfo& e = *it;
if (!e.has_pose)
continue;

Expand All @@ -163,7 +163,7 @@ int main(int argc, char** argv){
comp.p = robot_pose.getOrigin();
std::sort(entities.begin(), entities.end(), comp);

ed::EntityInfo e_target;
ed_msgs::EntityInfo e_target;
if (getTargetEntityBasedOnLastSeen(entities, e_target))
{
// std::cout << "Found target with id: " << e_target.id << std::endl;
Expand Down