// %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();
}
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();
}
// %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();
}
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);
}
// %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();
}
// %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 );
}
