diff --git a/asift_match/src/ROS_matcher.cpp b/asift_match/src/ROS_matcher.cpp index ae8ebc4..22bd100 100644 --- a/asift_match/src/ROS_matcher.cpp +++ b/asift_match/src/ROS_matcher.cpp @@ -5,6 +5,7 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT) std::string center_topic, image_topic, pointcloud_topic, reference_txt_path; std::vector reference_data_paths; + //Load Param _nh.param("tracked_object", tracked_object, "6DOF"); _nh.param("num_tilt", _num_tilt, 8); @@ -12,8 +13,10 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT) _nh.param("image_topic", image_topic,"/camera/rgb/image_raw"); _nh.param("pointcloud_topic", pointcloud_topic,"/camera/depth_registered/points"); + //Publisher _center_pub = _nh.advertise(center_topic, 1); + //Subscriber // info_sub = new message_filters::Subscriber(_nh, "camera/rgb/camera_info", 1); image_sub = new message_filters::Subscriber(_nh, image_topic, 1); pointcloud_sub= new message_filters::Subscriber(_nh, pointcloud_topic, 1); @@ -21,6 +24,7 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT) Timesync = new message_filters::Synchronizer(MySyncPolicy(2), *image_sub, *pointcloud_sub); Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2)); + //Load References if(_nh.getParam("reference_txt_path", reference_txt_path)) { if(matcher.loadReferences(reference_txt_path.c_str())) diff --git a/rviz_interface/rviz_interface/CMakeLists.txt b/rviz_interface/rviz_interface/CMakeLists.txt index 7de1a9e..f0dd748 100644 --- a/rviz_interface/rviz_interface/CMakeLists.txt +++ b/rviz_interface/rviz_interface/CMakeLists.txt @@ -9,6 +9,11 @@ project(rviz_interface) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS interactive_markers roscpp visualization_msgs tf rospy std_msgs message_generation genmsg) +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + ################################################ ## Declare ROS messages, services and actions ## ################################################ diff --git a/rviz_interface/rviz_interface/launch/interface_matching.launch b/rviz_interface/rviz_interface/launch/interface_matching.launch index a8720c0..bc33949 100644 --- a/rviz_interface/rviz_interface/launch/interface_matching.launch +++ b/rviz_interface/rviz_interface/launch/interface_matching.launch @@ -1,5 +1,24 @@ + + + + + + - + + + + + + + + + + + + diff --git a/rviz_interface/rviz_interface/launch/interface_matching.launch~ b/rviz_interface/rviz_interface/launch/interface_matching.launch~ index 93b40f2..d20bc5a 100644 --- a/rviz_interface/rviz_interface/launch/interface_matching.launch~ +++ b/rviz_interface/rviz_interface/launch/interface_matching.launch~ @@ -1,5 +1,24 @@ - - + + + + + + + + + + + + + + + + + + + diff --git a/rviz_interface/rviz_interface/setup.py b/rviz_interface/rviz_interface/setup.py new file mode 100644 index 0000000..85be25c --- /dev/null +++ b/rviz_interface/rviz_interface/setup.py @@ -0,0 +1,17 @@ +#!/usr/bin/python +# -*- coding: utf-8 -*- + +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['asift_matching'], + package_dir={'': 'src'}, + ) + +setup(**setup_args) + + diff --git a/rviz_interface/rviz_interface/src/RvizInterface.cpp b/rviz_interface/rviz_interface/src/RvizInterface.cpp index cbc8cf5..89040a0 100644 --- a/rviz_interface/rviz_interface/src/RvizInterface.cpp +++ b/rviz_interface/rviz_interface/src/RvizInterface.cpp @@ -3,20 +3,30 @@ //Constructeur RvizInterface::RvizInterface(): _server("RvizInterface") { + std::string objective_topic, vizualization_topic, config_topic, position_topic; + //Need to load multiple object + std::string object_name; + + //Load Param + _n.param("objective_topic", objective_topic,"/RvizInterface/state_objective"); + _n.param("vizualization_topic", vizualization_topic,"/RvizInterface/visual_marker"); + _n.param("config_topic", config_topic,"/RvizInterface/interface_config"); + _n.param("object_center_topic", position_topic,"/object_center"); + _n.param("tracked_object", object_name,"Object"); + //Topic you want to publish - _objective_pub = _n.advertise("/RvizInterface/state_objective", 10); - _visualization_pub = _n.advertise("/RvizInterface/visual_marker", 10); + _objective_pub = _n.advertise(objective_topic, 10); + _visualization_pub = _n.advertise(vizualization_topic, 10); //Topic you want to subscribe - _config_sub = _n.subscribe("/RvizInterface/interface_config", 1, &RvizInterface::configCallback, this); - _position_sub = _n.subscribe("/object_center", 1, &RvizInterface::positionCallback, this); + _config_sub = _n.subscribe(config_topic, 1, &RvizInterface::configCallback, this); + _position_sub = _n.subscribe(position_topic, 1, &RvizInterface::positionCallback, this); - _objects.push_back(new InteractiveObject(&_server, "6DOF", (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0))); + _objects.push_back(new InteractiveObject(&_server, object_name, (int) rviz_interface::StateSpace::STATE_3D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(0,0,0))); _objects[0]->add6DOFcontrol(); - _objects.push_back(new InteractiveObject(&_server, "3DOF", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(1,1,0))); - _objects[1]->add3DOFcontrol(); - + // _objects.push_back(new InteractiveObject(&_server, "3DOF", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(1,1,0))); + // _objects[1]->add3DOFcontrol(); // _objects.push_back(new InteractiveObject(&_server, "3DOF 2", (int) rviz_interface::StateSpace::STATE_2D, (int) visualization_msgs::Marker::SPHERE, tf::Vector3(-1,1,0))); // _objects[2]->add3DOFcontrol(tf::Vector3(0.5,0.5,0));