36 #define BOOST_PARAMETER_MAX_ARITY 7 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> 48 Eigen::Quaternionf(0.108755, 0.088921, 0.108755, 0.984092),
49 Eigen::Vector3f(0.3, 0.3, 0.3));
52 void processFeedbackCB(
const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
54 Eigen::Vector3f
p(feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z);
58 geometry_msgs::PointStamped ps;
59 ps.header = feedback->header;
66 int main(
int argc,
char** argv)
68 ros::init(argc, argv,
"cube_nearest_point");
70 pub_nearest_point = nh.
advertise<geometry_msgs::PointStamped>(
"nearest_point", 1);
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;
98 control.name =
"move_x";
99 control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
100 int_marker.controls.push_back(control);
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);
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);
118 server.applyChanges();
122 box.header.frame_id =
"world";
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);
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)
ROSCPP_DECL void spin(Spinner &spinner)
ros::Publisher pub_nearest_point
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()
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)