door_foot.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <interactive_markers/tools.h>
3 #include <jsk_interactive_marker/door_foot.h>
4 #include <jsk_interactive_marker/interactive_marker_utils.h>
5 
6 using namespace std;
7 
8 
9 visualization_msgs::Marker DoorFoot::makeRWallMarker(){
10  visualization_msgs::Marker marker;
11  marker.type = visualization_msgs::Marker::CUBE;
12  marker.scale.x = 0.05;
13  marker.scale.y = 0.7;
14  marker.scale.z = 2.0;
15 
16  marker.pose.position.y = - marker.scale.y / 2;
17  marker.pose.position.z = marker.scale.z / 2;
18  marker.pose.orientation.w = 1.0;
19 
20  marker.color.r = 0.0;
21  marker.color.g = 1.0;
22  marker.color.b = 0.0;
23  marker.color.a = 0.9;
24 
25  return marker;
26 }
27 
28 visualization_msgs::Marker DoorFoot::makeLWallMarker(){
29  visualization_msgs::Marker marker;
30  marker.type = visualization_msgs::Marker::CUBE;
31  marker.scale.x = 0.05;
32  marker.scale.y = 0.7;
33  marker.scale.z = 2.0;
34 
35  marker.pose.position.y = 0.914 + marker.scale.y / 2;
36  marker.pose.position.z = marker.scale.z / 2;
37  marker.pose.orientation.w = 1.0;
38 
39  marker.color.r = 0.0;
40  marker.color.g = 1.0;
41  marker.color.b = 0.0;
42  marker.color.a = 0.9;
43 
44  return marker;
45 }
46 
47 
48 visualization_msgs::Marker DoorFoot::makeDoorMarker(){
49  visualization_msgs::Marker marker;
50  marker.type = visualization_msgs::Marker::CUBE;
51  marker.scale.x = 0.05;
52  marker.scale.y = 0.914;
53  marker.scale.z = 2.0;
54 
55  marker.pose.position.y = marker.scale.y / 2;
56  marker.pose.position.z = marker.scale.z / 2;
57  marker.pose.orientation.w = 1.0;
58 
59  marker.color.r = 1.0;
60  marker.color.g = 1.0;
61  marker.color.b = 0.0;
62  marker.color.a = 0.3;
63 
64  return marker;
65 }
66 
67 visualization_msgs::Marker DoorFoot::makeKnobMarker(){
68  visualization_msgs::Marker marker;
69  marker.type = visualization_msgs::Marker::CUBE;
70  marker.scale.x = 0.03;
71  marker.scale.y = 0.1;
72  marker.scale.z = 0.03;
73 
74  marker.pose.position.x = -0.05;
75  marker.pose.position.y = 0.914 - 0.1 - marker.scale.y/2 ;
76  marker.pose.position.z = 0.9398;//37 in
77  marker.pose.orientation.w = 1.0;
78 
79  marker.color.r = 1.0;
80  marker.color.g = 0.0;
81  marker.color.b = 0.0;
82  marker.color.a = 1.0;
83 
84  return marker;
85 }
86 
87 visualization_msgs::Marker DoorFoot::makeKnobMarker(int position){
88  double size = 0.02;
89  visualization_msgs::Marker marker;
90  marker.type = visualization_msgs::Marker::CUBE;
91  marker.scale.x = 0.02;
92  marker.scale.y = size;
93  marker.scale.z = 0.03;
94 
95  marker.pose.position.x = -0.07;
96  marker.pose.position.y = 0.914 - marker.scale.y/2 - position * size;
97  marker.pose.position.z = 0.9398;//37 in
98  marker.pose.orientation.w = 1.0;
99 
100  switch(position % 5){
101  case 0:
102  marker.color.r = 1.0;
103  marker.color.g = 0.0;
104  marker.color.b = 0.0;
105  break;
106  case 1:
107  marker.color.r = 0.5;
108  marker.color.g = 0.0;
109  marker.color.b = 0.5;
110  break;
111  case 2:
112  marker.color.r = 0.0;
113  marker.color.g = 0.0;
114  marker.color.b = 1.0;
115  break;
116  case 3:
117  marker.color.r = 0.0;
118  marker.color.g = 0.5;
119  marker.color.b = 0.7;
120  break;
121  case 4:
122  marker.color.r = 0.0;
123  marker.color.g = 1.0;
124  marker.color.b = 0.0;
125  break;
126  }
127  marker.color.a = 0.7;
128 
129  return marker;
130 }
131 
132 
133 visualization_msgs::Marker DoorFoot::makeRFootMarker(){
134  geometry_msgs::Pose pose;
135  if(push){
136  //Right Foot Position
137  pose.position.x = -0.523;
138  pose.position.y = 0.270;
139  pose.position.z = 0;
140  pose.orientation.w = 0.766;
141  pose.orientation.x = 0;
142  pose.orientation.y = 0;
143  pose.orientation.z = 0.642788;
144  }else{
145  pose.position.x = -0.8;
146  pose.position.y = 0.0;
147  pose.position.z = -0.910;
148  pose.orientation.w = 1.0;
149  }
150 
151  return makeFootMarker(pose, true);
152 }
153 visualization_msgs::Marker DoorFoot::makeLFootMarker(){
154  geometry_msgs::Pose pose;
155  if(push){
156  pose.position.x = -0.747;
157  pose.position.y = 0.310;
158  pose.position.z = 0;
159  pose.orientation.w = 0.766;
160  pose.orientation.x = 0;
161  pose.orientation.y = 0;
162  pose.orientation.z = 0.642788;
163  }else{
164  pose.position.x = -0.8;
165  pose.position.y = -0.3;
166  pose.position.z = -0.910;
167  pose.orientation.w = 1.0;
168  }
169  return makeFootMarker(pose, false);
170 
171 }
172 
173 
174 visualization_msgs::Marker DoorFoot::makeFootMarker(geometry_msgs::Pose pose, bool right){
175  visualization_msgs::Marker marker;
176  marker.type = visualization_msgs::Marker::CUBE;
177  double PADDING_PARAM = 0.01;
178  marker.scale.x = 0.27 + PADDING_PARAM;
179  marker.scale.y = 0.14 + PADDING_PARAM;
180  marker.scale.z = 0.05;
181 
182  marker.pose = pose;
183 
184  if(right){
185  marker.color.r = 1.0;
186  marker.color.g = 1.0;
187  marker.color.b = 0.0;
188  }else{
189  marker.color.r = 1.0;
190  marker.color.g = 0.0;
191  marker.color.b = 1.0;
192  }
193  marker.color.a = 1.0;
194 
195  return marker;
196 }
197 
198 
199 visualization_msgs::InteractiveMarker DoorFoot::makeInteractiveMarker(){
200  visualization_msgs::InteractiveMarker mk;
201  mk.header.frame_id = "/map";
202  mk.header.stamp = ros::Time(0);
203  mk.name = marker_name;
204  mk.scale = 0.3;
205  mk.pose = door_pose;
206 
207  visualization_msgs::InteractiveMarkerControl triangleMarker;
208  triangleMarker.always_visible = true;
209  triangleMarker.markers.push_back( makeRWallMarker());
210  triangleMarker.markers.push_back( makeLWallMarker());
211  triangleMarker.markers.push_back( makeDoorMarker());
212 
213  if(use_color_knob){
214  for(int i=0;i<12;i++){
215  triangleMarker.markers.push_back( makeKnobMarker(i));
216  }
217  }else{
218  triangleMarker.markers.push_back( makeKnobMarker());
219  }
220 
221  if (footstep_show_initial_p_) {
222  for(size_t i=0; i<2; i++){
223  if(foot_list[i].header.frame_id == "right"){
224  triangleMarker.markers.push_back( makeFootMarker( foot_list[i].pose, true ));
225  }else{
226  triangleMarker.markers.push_back( makeFootMarker( foot_list[i].pose, false ));
227  }
228  }
229  }
230  else {
231  int first_index = footstep_index_ * 2;
232  int second_index = footstep_index_ * 2 + 1;
233  if (foot_list.size() > first_index) {
234  triangleMarker.markers.push_back( makeFootMarker( foot_list[first_index].pose,
235  foot_list[first_index].header.frame_id == "right"));
236  }
237  if (foot_list.size() > second_index) {
238  triangleMarker.markers.push_back( makeFootMarker( foot_list[second_index].pose,
239  foot_list[second_index].header.frame_id == "right"));
240  }
241  }
242 
243 
244 
245  mk.controls.push_back( triangleMarker );
246 
247  im_helpers::add6DofControl(mk, true);
248  return mk;
249 }
250 
251 void DoorFoot::moveBoxCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
252 {
253  door_pose = feedback->pose;
254  // std::cout << "moved" << std::endl;
255 }
256 
257 void DoorFoot::showStandLocationCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
258  footstep_show_initial_p_ = true;
259  updateBoxInteractiveMarker();
260 }
261 void DoorFoot::showNextStepCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
262  if (foot_list.size() > 2) {
263  if (footstep_show_initial_p_) {
264  footstep_index_ = 1;
265  footstep_show_initial_p_ = false;
266  }
267  else {
268  ++footstep_index_;
269  if (foot_list.size() / 2 == footstep_index_) {
270  footstep_index_ = 1;
271  }
272  }
273  }
274  updateBoxInteractiveMarker();
275 }
276 void DoorFoot::showPreviousStepCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
277  if (foot_list.size() > 2) {
278  if (footstep_show_initial_p_) {
279  footstep_index_ = 1;
280  footstep_show_initial_p_ = false;
281  }
282  else {
283  --footstep_index_;
284  if (footstep_index_ == 0) {
285  footstep_index_ = (foot_list.size() - 1)/ 2;
286  }
287  }
288  }
289  updateBoxInteractiveMarker();
290 }
291 
292 void DoorFoot::pushDoorCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
293  push = true;
294  updateBoxInteractiveMarker();
295 }
296 
297 void DoorFoot::pullDoorCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
298  push = false;
299  updateBoxInteractiveMarker();
300 }
301 
302 
303 interactive_markers::MenuHandler DoorFoot::makeMenuHandler(){
304  interactive_markers::MenuHandler mh;
305  mh.insert("Push Door", boost::bind( &DoorFoot::pushDoorCb, this, _1));
306  mh.insert("Pull Door", boost::bind( &DoorFoot::pullDoorCb, this, _1));
307  mh.insert("Show Initial Stand Location", boost::bind( &DoorFoot::showStandLocationCb, this, _1));
308  mh.insert("Show Next Step", boost::bind( &DoorFoot::showNextStepCb, this, _1));
309  mh.insert("Show Previous Step", boost::bind( &DoorFoot::showPreviousStepCb, this, _1));
310  return mh;
311 }
312 
313 
314 void DoorFoot::updateBoxInteractiveMarker(){
315  visualization_msgs::InteractiveMarker boxIM = makeInteractiveMarker();
316 
317  server_->insert(boxIM,
318  boost::bind( &DoorFoot::moveBoxCb, this, _1 ));
319  menu_handler.apply(*server_, marker_name);
320  server_->applyChanges();
321 }
322 
323 DoorFoot::DoorFoot () : nh_(), pnh_("~") {
324  footstep_show_initial_p_ = true;
325  footstep_index_ = -1;
326  pnh_.param("server_name", server_name, std::string ("") );
327  pnh_.param("size", size_, 1.0 );
328  pnh_.param("marker_name", marker_name, std::string ("door_marker") );
329  pnh_.param("push", push, true);
330  pnh_.param("use_color_knob", use_color_knob, true);
331 
332  XmlRpc::XmlRpcValue v;
333  pnh_.param("foot_list", v, v);
334  for(int i=0; i< v.size(); i++){
335  XmlRpc::XmlRpcValue foot = v[i];
336  geometry_msgs::Pose p = getPose(foot["pose"]);
337  geometry_msgs::PoseStamped footPose;
338  footPose.pose = p;
339  footPose.header.frame_id.assign(foot["leg"]);
340  foot_list.push_back(footPose);
341  }
342 
343  if ( server_name == "" ) {
344  server_name = ros::this_node::getName();
345  }
346  server_.reset( new interactive_markers::InteractiveMarkerServer(server_name));
347 
348  menu_handler = makeMenuHandler();
349  updateBoxInteractiveMarker();
350  return;
351 }
352 
353 
354 int main(int argc, char** argv)
355 {
356  ros::init(argc, argv, "door_foot_marker");
357  DoorFoot triFoot;
358  int counter = 0;
359  ros::spin();
360  return 0;
361 }
362 
363 
364 geometry_msgs::Pose getPose( XmlRpc::XmlRpcValue val){
365  geometry_msgs::Pose p;
366  XmlRpc::XmlRpcValue pos = val["position"];
367  p.position.x = getXmlValue(pos["x"]);
368  p.position.y = getXmlValue(pos["y"]);
369  p.position.z = getXmlValue(pos["z"]);
370 
371  XmlRpc::XmlRpcValue ori = val["orientation"];
372  p.orientation.x = getXmlValue(ori["x"]);
373  p.orientation.y = getXmlValue(ori["y"]);
374  p.orientation.z = getXmlValue(ori["z"]);
375  p.orientation.w = getXmlValue(ori["w"]);
376 
377  return p;
378 }
379 
380 double getXmlValue( XmlRpc::XmlRpcValue val ){
381  switch(val.getType()){
382  case XmlRpc::XmlRpcValue::TypeInt:
383  return (double)((int)val);
384  case XmlRpc::XmlRpcValue::TypeDouble:
385  return (double)val;
386  default:
387  return 0;
388  }
389 }
390 
DoorFoot::pnh_
ros::NodeHandle pnh_
Definition: door_foot.h:37
DoorFoot::menu_handler
interactive_markers::MenuHandler menu_handler
Definition: door_foot.h:43
DoorFoot::server_name
std::string server_name
Definition: door_foot.h:40
DoorFoot::push
bool push
Definition: door_foot.h:45
pos
pos
counter
int counter
XmlRpc::XmlRpcValue::size
int size() const
interactive_marker_utils.h
DoorFoot::showNextStepCb
void showNextStepCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: door_foot.cpp:261
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
DoorFoot::makeLWallMarker
visualization_msgs::Marker makeLWallMarker()
Definition: door_foot.cpp:28
getXmlValue
double getXmlValue(XmlRpc::XmlRpcValue val)
Definition: door_foot.cpp:380
interactive_markers::MenuHandler
interactive_markers::InteractiveMarkerServer
XmlRpc::XmlRpcValue::getType
Type const & getType() const
DoorFoot::makeRFootMarker
visualization_msgs::Marker makeRFootMarker()
Definition: door_foot.cpp:133
pose
pose
DoorFoot::makeFootMarker
visualization_msgs::Marker makeFootMarker(geometry_msgs::Pose pose, bool right)
Definition: door_foot.cpp:174
main
int main(int argc, char **argv)
Definition: door_foot.cpp:354
DoorFoot::pullDoorCb
void pullDoorCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: door_foot.cpp:297
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
size
size
DoorFoot::updateBoxInteractiveMarker
void updateBoxInteractiveMarker()
Definition: door_foot.cpp:314
DoorFoot::footstep_show_initial_p_
bool footstep_show_initial_p_
Definition: door_foot.h:34
DoorFoot::showPreviousStepCb
void showPreviousStepCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: door_foot.cpp:276
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
DoorFoot::footstep_index_
int footstep_index_
Definition: door_foot.h:35
header
header
getPose
geometry_msgs::Pose getPose(XmlRpc::XmlRpcValue val)
Definition: door_foot.cpp:364
DoorFoot::makeInteractiveMarker
visualization_msgs::InteractiveMarker makeInteractiveMarker()
Definition: door_foot.cpp:199
DoorFoot::moveBoxCb
void moveBoxCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: door_foot.cpp:251
DoorFoot::use_color_knob
bool use_color_knob
Definition: door_foot.h:46
DoorFoot::makeDoorMarker
visualization_msgs::Marker makeDoorMarker()
Definition: door_foot.cpp:48
DoorFoot::pushDoorCb
void pushDoorCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: door_foot.cpp:292
interactive_markers::MenuHandler::insert
EntryHandle insert(const std::string &title, const FeedbackCallback &feedback_cb)
DoorFoot::foot_list
std::vector< geometry_msgs::PoseStamped > foot_list
Definition: door_foot.h:47
door_foot.h
p
p
DoorFoot::size_
double size_
Definition: door_foot.h:44
im_helpers::add6DofControl
void add6DofControl(visualization_msgs::InteractiveMarker &msg, bool fixed=false)
Definition: interactive_marker_helpers.cpp:132
v
GLfloat v[8][3]
DoorFoot::makeLFootMarker
visualization_msgs::Marker makeLFootMarker()
Definition: door_foot.cpp:153
DoorFoot::showStandLocationCb
void showStandLocationCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: door_foot.cpp:257
DoorFoot::server_
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
Definition: door_foot.h:38
DoorFoot::makeMenuHandler
interactive_markers::MenuHandler makeMenuHandler()
Definition: door_foot.cpp:303
marker
marker
DoorFoot::marker_name
std::string marker_name
Definition: door_foot.h:41
DoorFoot::makeKnobMarker
visualization_msgs::Marker makeKnobMarker()
Definition: door_foot.cpp:67
DoorFoot::makeRWallMarker
visualization_msgs::Marker makeRWallMarker()
Definition: door_foot.cpp:9

jsk_interactive_marker
Author(s): furuta
autogenerated on Sat Mar 20 2021 03:03:33