Пример #1
0
// %Tag(main)%
int main(int argc, char** argv)
{
  ros::init(argc, argv, "basic_controls");
  ros::NodeHandle n;

  // create a timer to update the published transforms
  ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);

  server.reset( new interactive_markers::InteractiveMarkerServer("basic_controls","",false) );

  ros::Duration(0.1).sleep();

  menu_handler.insert( "First Entry", &processFeedback );
  menu_handler.insert( "Second Entry", &processFeedback );
  interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" );
  menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback );
  menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback );

  make6DofMarker( false );
  make6DofMarker( true );
  makeRandomDofMarker( );
  makeViewFacingMarker( );
  makeQuadrocopterMarker( );
  makeChessPieceMarker( );
  makePanTiltMarker( );
  makeMenuMarker( );
  makeMovingMarker( );

  server->applyChanges();

  ros::spin();

  server.reset();
}
Пример #2
0
int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init(argc, argv, "interactive_marker_test");
  ros::NodeHandle n;

  // create a timer to update the published transforms
  ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);

  server.reset( new interactive_markers::InteractiveMarkerServer("interactive_marker_test","",false) );

  ros::Duration(0.1).sleep();

  menu_handler.insert( "First Entry", &processFeedback );
  menu_handler.insert( "Second Entry", &processFeedback );
  interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" );
  menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback );
  menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback );


  makeButtonMarker( );


  server->applyChanges();

  ros::spin();

  server.reset();
}
Пример #3
0
// %Tag(main)%
int main(int argc, char** argv)
{
  ros::init(argc, argv, "basic_controls");
  ros::NodeHandle n;

  // create a timer to update the published transforms
  ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);

  server.reset( new interactive_markers::InteractiveMarkerServer("basic_controls","",false) );

  ros::Duration(0.1).sleep();

  menu_handler.insert( "First Entry", &processFeedback );
  menu_handler.insert( "Second Entry", &processFeedback );
  interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Submenu" );
  menu_handler.insert( sub_menu_handle, "First Entry", &processFeedback );
  menu_handler.insert( sub_menu_handle, "Second Entry", &processFeedback );

  tf::Vector3 position;
  position = tf::Vector3(-3, 3, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::NONE, position, true );
  position = tf::Vector3( 0, 3, 0);
  make6DofMarker( true, visualization_msgs::InteractiveMarkerControl::NONE, position, true );
  position = tf::Vector3( 3, 3, 0);
  makeRandomDofMarker( position );
  position = tf::Vector3(-3, 0, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::ROTATE_3D, position, false );
  position = tf::Vector3( 0, 0, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D, position, true );
  position = tf::Vector3( 3, 0, 0);
  make6DofMarker( false, visualization_msgs::InteractiveMarkerControl::MOVE_3D, position, false );
  position = tf::Vector3(-3,-3, 0);
  makeViewFacingMarker( position );
  position = tf::Vector3( 0,-3, 0);
  makeQuadrocopterMarker( position );
  position = tf::Vector3( 3,-3, 0);
  makeChessPieceMarker( position );
  position = tf::Vector3(-3,-6, 0);
  makePanTiltMarker( position );
  position = tf::Vector3( 0,-6, 0);
  makeMovingMarker( position );
  position = tf::Vector3( 3,-6, 0);
  makeMenuMarker( position );
  position = tf::Vector3( 0,-9, 0);
  makeButtonMarker( position );

  server->applyChanges();

  ros::spin();

  server.reset();
}
Пример #4
0
int main(int argc, char** argv) {
	ros::init(argc, argv, "GUI_planner");
	ros::NodeHandle n;
	cmove_ = new ClopemaMove;
	setPlanningScene();
	server.reset(new interactive_markers::InteractiveMarkerServer("basic_controls", "", false));
	vis_marker_array_publisher_ = n.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);

	ros::Duration(0.5).sleep();

	menu_handler.insert("Plan", &planTrajectory);
	menu_handler.insert("Visualize trajectory", &visualizeTrajectory);
	menu_handler.insert("Execute trajectory", &executeTrajectory);
	menu_handler.insert("Set to the home position", &setHomePosition);
	interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert("Servo Power");
	menu_handler.insert(sub_menu_handle, "Turn off", &setServoPowerOff);
	menu_handler.insert(sub_menu_handle, "Turn off - force!", &setServoPowerOffForce);
	menu_handler.insert("Plan & Visualize & Execute & Turn off", &planVisExeOff);

	makeBox(1);
	makeBox(2);
	makeMenuMarker();
	publishRobotState();

	server->applyChanges();
	ros::spin();
	server.reset();
	return stop(1);
}
Пример #5
0
// %Tag(Menu)%
void makeMenuMarker()
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "/base_link";
  int_marker.pose.position.y = -3.0 * marker_pos++;;
  int_marker.scale = 1;

  int_marker.name = "context_menu";
  int_marker.description = "Context Menu\n(Right Click)";

  InteractiveMarkerControl control;

  // make one control using default visuals
  control.interaction_mode = InteractiveMarkerControl::MENU;
  control.description="Options";
  control.name = "menu_only_control";
  int_marker.controls.push_back(control);

  // make one control showing a box
  Marker marker = makeBox( int_marker );
  control.markers.push_back( marker );
  control.always_visible = true;
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
  menu_handler.apply( *server, int_marker.name );
}
int main(int argc, char **argv) {
	ros::init(argc, argv, "plan_out_of_vision_tutorial");
	ros::NodeHandle nh;

	server.reset(new interactive_markers::InteractiveMarkerServer("vision_controls", "", false));
	menu_handler.insert("Visualize vision", &visualize);
	menu_handler.insert("Go out of vision - r1_arm", &planR1);
	menu_handler.insert("Go out of vision - r2_arm", &planR2);
	menu_handler.insert("Go out of vision - both", &plan);
	makeMenuMarker();

	server->applyChanges();
	ros::spin();
	server.reset();
	return 1;
}
void makeMenuMarker() {
	InteractiveMarker int_marker;
	int_marker.header.frame_id = "/base_link";
	int_marker.pose.position.z = 2.5;
	int_marker.scale = 1;
	int_marker.name = "context_menu";
	int_marker.description = "Vision Menu";
	InteractiveMarkerControl control;
	control.interaction_mode = InteractiveMarkerControl::MENU;
	control.description = "Options";
	control.name = "menu_only_control";

	Marker marker;
	marker.type = Marker::SPHERE;
	marker.scale.x = 0.45;
	marker.scale.y = 0.45;
	marker.scale.z = 0.45;
	marker.color.r = 0.0;
	marker.color.g = 0.0;
	marker.color.b = 1.0;
	marker.color.a = 1.0;

	control.markers.push_back(marker);
	control.always_visible = true;
	int_marker.controls.push_back(control);

	server->insert(int_marker);
	menu_handler.apply(*server, int_marker.name);
}
// %Tag(Menu)%
void makeMenuMarker( const tf::Vector3& position )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "/l_foot";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "Command Menu";
  int_marker.description = "Command Menu\n(Right Click)";

  InteractiveMarkerControl control;

  control.interaction_mode = InteractiveMarkerControl::MENU;
  control.name = "menu_only_control";

  Marker marker = makeManuBox();
  control.markers.push_back( marker );
  //control.always_visible = true;
  int_marker.controls.push_back(control);

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
  menu_handler.apply( *server, int_marker.name );
}
// %Tag(main)%
int main(int argc, char** argv)
{
  ros::init(argc, argv, "foot_interface", ros::init_options::NoSigintHandler);
  ros::NodeHandle n;
  server.reset( new interactive_markers::InteractiveMarkerServer("foot_interface","",false) );

  ros::Duration(0.1).sleep();
  initTime = ros::Time::now();


  menu_handler.insert("Clean the Path and Refresh the point cloud", &processFeedback );


  interactive_markers::MenuHandler::EntryHandle sub_menu_handle = menu_handler.insert( "Foot step planning part" );
  menu_handler.insert( sub_menu_handle, "Publish Goal Location and A* plan", &processFeedback );
  menu_handler.insert( sub_menu_handle, "Publish Goal Location and Align with block", &processFeedback );
  
  interactive_markers::MenuHandler::EntryHandle sub_menu_handle1 = menu_handler.insert( "Step for Cinder block" );
  menu_handler.insert( sub_menu_handle1, "Plan walk up cinder block", &processFeedback );
  menu_handler.insert( sub_menu_handle1, "Plan walk down cinder block", &processFeedback );
  interactive_markers::MenuHandler::EntryHandle sub_menu_handle2 = menu_handler.insert( "Small foot adjustment" );
  menu_handler.insert( sub_menu_handle2, "small forward", &processFeedback );
  menu_handler.insert( sub_menu_handle2, "small left side walk", &processFeedback );
  menu_handler.insert( sub_menu_handle2, "small right side walk", &processFeedback );
  menu_handler.insert( "Publish the Foot Sequence to ATLAS", &processFeedback );
  
  interactive_markers::MenuHandler::EntryHandle sub_menu_handle3 = menu_handler.insert( "Walking Mode" );
  menu_handler.insert( sub_menu_handle3, "ramp", &processFeedback );
  menu_handler.insert( sub_menu_handle3, "zig_zag", &processFeedback );
  menu_handler.insert( sub_menu_handle3, "walk up cinder block", &processFeedback );
  menu_handler.insert( sub_menu_handle3, "walk down cinder block", &processFeedback );
 
  makeMovingMarker_Goal();
  tf::Vector3 p;
  p = tf::Vector3(0, -1, 0);
  makeMenuMarker(p);
  

      geometry_msgs::Pose position;
      position.position.x = 0+0.045;
      position.position.y = 0;
      position.position.z = 0;
      position.orientation.x = 0;
      position.orientation.y = 0;
      position.orientation.z = 0;
      position.orientation.w = 1;

    std::stringstream ss;
    ss << 1;
    std::string s = ss.str();
  makeMovingMarker(position, s, false, false);
  server->applyChanges();

  goal_location_pub_ = n.advertise<interactive_walk_planner_new::goal_pose>("/interactive_walk_planner_new/goal_pose", 100, true );
  task_mode_pub_ = n.advertise<interactive_walk_planner_new::task_mode>("/interactive_walk_planner_new/task_mode", 100, true );
  replan_message_pub_ = n.advertise<interactive_walk_planner_new::replan>("/interactive_walk_planner_new/replan_message", 100, true );
  ros::Subscriber sub_foot_sequence = n.subscribe<interactive_walk_planner_new::foot_sequence>( "/atlas/planned_foot_sequence", 100, &foot_sequenceCallback );

  //ros::Subscriber sub_ori_estimation = n.subscribe<interactive_walk_planner_new::orientation>( "/orientation_estimation/orientation", 100, &ori_estimationCallback );

  pub_wrecs_step = n.advertise<atlas_ros_msgs::AtlasStepData>("/ocu/add_step_relative_left_foot", 100);


  ros::spin();
  server.reset();
}
Пример #10
0
// %Tag(6DOF)%
void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector3& position, bool show_6dof )
{
  InteractiveMarker int_marker;
  int_marker.header.frame_id = "base_link";
  tf::pointTFToMsg(position, int_marker.pose.position);
  int_marker.scale = 1;

  int_marker.name = "simple_6dof";
  int_marker.description = "Simple 6-DOF Control";

  // insert a box
  makeBoxControl(int_marker);
  int_marker.controls[0].interaction_mode = interaction_mode;

  InteractiveMarkerControl control;

  if ( fixed )
  {
    int_marker.name += "_fixed";
    int_marker.description += "\n(fixed orientation)";
    control.orientation_mode = InteractiveMarkerControl::FIXED;
  }

  if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE)
  {
      std::string mode_text;
      if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_3D )         mode_text = "MOVE_3D";
      if( interaction_mode == visualization_msgs::InteractiveMarkerControl::ROTATE_3D )       mode_text = "ROTATE_3D";
      if( interaction_mode == visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D )  mode_text = "MOVE_ROTATE_3D";
      int_marker.name += "_" + mode_text;
      int_marker.description = std::string("3D Control") + (show_6dof ? " + 6-DOF controls" : "") + "\n" + mode_text;
  }

  if(show_6dof)
  {
    control.orientation.w = 1;
    control.orientation.x = 1;
    control.orientation.y = 0;
    control.orientation.z = 0;
    control.name = "rotate_x";
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_x";
    control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);

    control.orientation.w = 1;
    control.orientation.x = 0;
    control.orientation.y = 1;
    control.orientation.z = 0;
    control.name = "rotate_z";
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_z";
    control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);

    control.orientation.w = 1;
    control.orientation.x = 0;
    control.orientation.y = 0;
    control.orientation.z = 1;
    control.name = "rotate_y";
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    int_marker.controls.push_back(control);
    control.name = "move_y";
    control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);
  }

  server->insert(int_marker);
  server->setCallback(int_marker.name, &processFeedback);
  if (interaction_mode != visualization_msgs::InteractiveMarkerControl::NONE)
    menu_handler.apply( *server, int_marker.name );
}