AntoineH/BaxterInterface
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Ajout d'options au launch file (Interface Rviz)

Browse source
This commit is contained in:
Unknown 2018-08-03 15:27:30 +02:00
parent ced3ccd8cd
commit f4e33539ee
6 changed files with 85 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

4
asift_match/src/ROS_matcher.cpp
View file

@ -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<std::string> reference_data_paths;
//Load Param
_nh.param<std::string>("tracked_object", tracked_object, "6DOF");
_nh.param<int>("num_tilt", _num_tilt, 8);
@ -12,8 +13,10 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT)
_nh.param<std::string>("image_topic", image_topic,"/camera/rgb/image_raw");
_nh.param<std::string>("pointcloud_topic", pointcloud_topic,"/camera/depth_registered/points");
//Publisher
_center_pub = _nh.advertise<rviz_interface::NamedPoint>(center_topic, 1);
//Subscriber
// info_sub = new message_filters::Subscriber<sensor_msgs::CameraInfo>(_nh, "camera/rgb/camera_info", 1);
image_sub = new message_filters::Subscriber<sensor_msgs::Image>(_nh, image_topic, 1);
pointcloud_sub= new message_filters::Subscriber<sensor_msgs::PointCloud2>(_nh, pointcloud_topic, 1);
@ -21,6 +24,7 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT)
Timesync = new message_filters::Synchronizer<MySyncPolicy>(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()))

5
rviz_interface/rviz_interface/CMakeLists.txt
View file

@ -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 ##
################################################

21
rviz_interface/rviz_interface/launch/interface_matching.launch
View file

@ -1,5 +1,24 @@
<launch>
<node name="rviz" pkg="rviz" type="rviz"/>
<!--Interface/-->
<param name="objective_topic" value="/RvizInterface/state_objective"/>
<param name="vizualization_topic" value="/RvizInterface/visual_marker"/>
<param name="config_topic" value="/RvizInterface/interface_config"/>
<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
<!--node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher"/-->
<!--Matcher/-->
<param name="tracked_object" value="Foo"/>
<param name="num_tilt" type="int" value="8"/>
<param name="reference_txt_path" value="$(find asift_matching)/reference_data/book_references.txt"/>
<!--rosparam param="reference_data">[
"$(find asift_matching)/reference_data/train_image_000.png",
"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam-->
<param name="object_center_topic" value="/object_center"/>
<param name="image_topic" value="/camera/rgb/image_raw"/>
<param name="pointcloud_topic" value="/camera/depth_registered/points"/>
<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher" output="screen"/>
</launch>

23
rviz_interface/rviz_interface/launch/interface_matching.launch~
View file

@ -1,5 +1,24 @@
<launch>
<node name="rviz" pkg="rviz" type="rviz"/>
<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher"/>
<!--Interface/-->
<param name="objective_topic" value="/RvizInterface/state_objective"/>
<param name="vizualization_topic" value="/RvizInterface/visual_marker"/>
<param name="config_topic" value="/RvizInterface/interface_config"/>
<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface" output="screen"/>
<!--Matcher/-->
<param name="tracked_object" value="Foo"/>
<param name="num_tilt" type="int" value="8"/>
<param name="reference_txt_path" value="$(find asift_matching)/reference_data/book_references.txt"/>
<!--rosparam param="reference_data">[
"$(find asift_matching)/reference_data/train_image_000.png",
"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam-->
<param name="object_center_topic" value="/object_center"/>
<param name="image_topic" value="/camera/rgb/image_raw"/>
<param name="pointcloud_topic" value="/camera/depth_registered/points"/>
<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher" output="screen"/>
</launch>

17
rviz_interface/rviz_interface/setup.py Normal file
View file

@ -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)

26
rviz_interface/rviz_interface/src/RvizInterface.cpp
View file

@ -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<std::string>("objective_topic", objective_topic,"/RvizInterface/state_objective");
_n.param<std::string>("vizualization_topic", vizualization_topic,"/RvizInterface/visual_marker");
_n.param<std::string>("config_topic", config_topic,"/RvizInterface/interface_config");
_n.param<std::string>("object_center_topic", position_topic,"/object_center");
_n.param<std::string>("tracked_object", object_name,"Object");
//Topic you want to publish
_objective_pub = _n.advertise<rviz_interface::StateSpace>("/RvizInterface/state_objective", 10);
_visualization_pub = _n.advertise<visualization_msgs::Marker>("/RvizInterface/visual_marker", 10);
_objective_pub = _n.advertise<rviz_interface::StateSpace>(objective_topic, 10);
_visualization_pub = _n.advertise<visualization_msgs::Marker>(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));