sample_cube_nearest_point.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
37 #include <ros/ros.h>
38 #include "jsk_pcl_ros/geo_util.h"
39 #include <jsk_recognition_msgs/BoundingBoxArray.h>
41 #include <visualization_msgs/Marker.h>
42 #include <visualization_msgs/MarkerArray.h>
43 #include <geometry_msgs/PointStamped.h>
44 
45 using namespace jsk_pcl_ros;
46 
47 Cube cube(Eigen::Vector3f(1, 0, 0),
48  Eigen::Quaternionf(0.108755, 0.088921, 0.108755, 0.984092),
49  Eigen::Vector3f(0.3, 0.3, 0.3));
50 
52 void processFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
53 {
54  Eigen::Vector3f p(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z);
55  double distance;
56  Eigen::Vector3f q = cube.nearestPoint(p, distance);
57  ROS_INFO("distance: %f", distance);
58  geometry_msgs::PointStamped ps;
59  ps.header = feedback->header;
60  ps.point.x = q[0];
61  ps.point.y = q[1];
62  ps.point.z = q[2];
63  pub_nearest_point.publish(ps);
64 }
65 
66 int main(int argc, char** argv)
67 {
68  ros::init(argc, argv, "cube_nearest_point");
69  ros::NodeHandle nh("~");
70  pub_nearest_point = nh.advertise<geometry_msgs::PointStamped>("nearest_point", 1);
71  interactive_markers::InteractiveMarkerServer server("sample_cube_nearest_point");
72 
73  visualization_msgs::InteractiveMarker int_marker;
74  int_marker.header.frame_id = "world";
75  int_marker.name = "marker";
76  int_marker.pose.orientation.w = 1.0;
77  int_marker.pose.position.x = -2.0;
78  visualization_msgs::Marker object_marker;
79  object_marker.type = visualization_msgs::Marker::SPHERE;
80  object_marker.scale.x = 0.1;
81  object_marker.scale.y = 0.1;
82  object_marker.scale.z = 0.1;
83  object_marker.color.r = 1.0;
84  object_marker.color.g = 1.0;
85  object_marker.color.b = 1.0;
86  object_marker.color.a = 1.0;
87  visualization_msgs::InteractiveMarkerControl object_marker_control;
88  object_marker_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
89  object_marker_control.always_visible = true;
90  object_marker_control.markers.push_back(object_marker);
91  int_marker.controls.push_back(object_marker_control);
92  visualization_msgs::InteractiveMarkerControl control;
93  control.orientation.w = 1;
94  control.orientation.x = 1;
95  control.orientation.y = 0;
96  control.orientation.z = 0;
97 
98  control.name = "move_x";
99  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
100  int_marker.controls.push_back(control);
101 
102  control.orientation.w = 1;
103  control.orientation.x = 0;
104  control.orientation.y = 1;
105  control.orientation.z = 0;
106  control.name = "move_z";
107  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
108  int_marker.controls.push_back(control);
109 
110  control.orientation.w = 1;
111  control.orientation.x = 0;
112  control.orientation.y = 0;
113  control.orientation.z = 1;
114  control.name = "move_y";
115  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
116  int_marker.controls.push_back(control);
117  server.insert(int_marker, &processFeedbackCB);
118  server.applyChanges();
119 
120  ros::Publisher pub_box_array = nh.advertise<jsk_recognition_msgs::BoundingBoxArray>("bbox_array", 1, true);
121  jsk_recognition_msgs::BoundingBox box = cube.toROSMsg();
122  box.header.frame_id = "world";
123  box.header.stamp = ros::Time::now();
124  jsk_recognition_msgs::BoundingBoxArray box_array;
125  box_array.boxes.push_back(box);
126  box_array.header = box.header;
127  pub_box_array.publish(box_array);
128  ros::spin();
129 }
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void processFeedbackCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
server
ROSCPP_DECL void spin(Spinner &spinner)
ros::Publisher pub_nearest_point
q
#define ROS_INFO(...)
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f &p, double &distance)
jsk_recognition_msgs::BoundingBox toROSMsg()
p
static Time now()
Cube cube(Eigen::Vector3f(1, 0, 0), Eigen::Quaternionf(0.108755, 0.088921, 0.108755, 0.984092), Eigen::Vector3f(0.3, 0.3, 0.3))
double distance(const urdf::Pose &transform)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47