├── .gitignore ├── Documents ├── Cplusplus_classes_in_ROS.pdf ├── Intro_to_ROS_v4.pdf ├── ROS_history.pdf ├── how_to_define_custom_messages.pdf ├── intro_to_ROS_services.pdf ├── intro_to_action_servers.pdf ├── intro_to_gazebo.pdf ├── intro_to_rviz.pdf ├── roadmap.pdf ├── robot_model_visualization.pdf └── simulating_sensors_in_gazebo.pdf ├── README.md ├── cwru_srv └── CMakeLists.txt ├── example_action_server ├── CMakeLists.txt ├── README.md ├── action │ └── demo.action ├── package.xml └── src │ ├── example_action_client.cpp │ └── example_action_server.cpp ├── example_interactive_marker ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ ├── IM_6dof.cpp │ └── IM_6dof_svc_client_test.cpp ├── example_ros_class ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ ├── example_ros_class.cpp │ └── example_ros_class.h ├── example_ros_msg ├── CMakeLists.txt ├── msg │ ├── example_message.msg │ └── example_message.msg~ ├── package.xml └── src │ └── example_ros_message_publisher.cpp ├── example_ros_service ├── CMakeLists.txt ├── README.md ├── package.xml ├── src │ ├── example_ros_client.cpp │ └── example_ros_service.cpp └── srv │ └── example_server_msg.srv ├── example_rviz_marker ├── CMakeLists.txt ├── package.xml └── src │ └── example_rviz_marker.cpp ├── example_srv ├── CMakeLists.txt ├── package.xml ├── src │ └── example_ros_service.cpp └── srv │ ├── IM_node_service_message.srv │ ├── arm_nav_service_message.srv │ ├── path_service_message.srv │ ├── simple_bool_service_message.srv │ ├── simple_float_service_message.srv │ └── simple_int_service_message.srv ├── minimal_joint_controller ├── CMakeLists.txt ├── package.xml └── src │ └── minimal_joint_controller.cpp ├── minimal_nodes ├── CMakeLists.txt ├── README.md ├── catkin_notes ├── launch │ └── minimal_nodes.launch ├── package.xml └── src │ ├── minimal_controller.cpp │ ├── minimal_publisher.cpp │ ├── minimal_simulator.cpp │ └── minimal_subscriber.cpp └── minimal_robot_description ├── CMakeLists.txt ├── minimal_robot.launch ├── minimal_robot_description.launch ├── minimal_robot_description.urdf ├── minimal_robot_visual.launch ├── minimal_robot_visual.urdf ├── minimal_robot_w_sensor.launch ├── minimal_robot_w_sensor.urdf └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | /catkin/devel/* 41 | *~ 42 | .catkin_workspace 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | *.cmake 49 | 50 | #temp files 51 | *~ 52 | -------------------------------------------------------------------------------- /Documents/Cplusplus_classes_in_ROS.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/Cplusplus_classes_in_ROS.pdf -------------------------------------------------------------------------------- /Documents/Intro_to_ROS_v4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/Intro_to_ROS_v4.pdf -------------------------------------------------------------------------------- /Documents/ROS_history.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/ROS_history.pdf -------------------------------------------------------------------------------- /Documents/how_to_define_custom_messages.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/how_to_define_custom_messages.pdf -------------------------------------------------------------------------------- /Documents/intro_to_ROS_services.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/intro_to_ROS_services.pdf -------------------------------------------------------------------------------- /Documents/intro_to_action_servers.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/intro_to_action_servers.pdf -------------------------------------------------------------------------------- /Documents/intro_to_gazebo.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/intro_to_gazebo.pdf -------------------------------------------------------------------------------- /Documents/intro_to_rviz.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/intro_to_rviz.pdf -------------------------------------------------------------------------------- /Documents/roadmap.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/roadmap.pdf -------------------------------------------------------------------------------- /Documents/robot_model_visualization.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/robot_model_visualization.pdf -------------------------------------------------------------------------------- /Documents/simulating_sensors_in_gazebo.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wsnewman/ros_class/ebe99dcd9671a392abe6dec3f966a402dd327c96/Documents/simulating_sensors_in_gazebo.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros_class 2 | minimal ros examples for ROS class 3 | -------------------------------------------------------------------------------- /cwru_srv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cwru_srv) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | #message_generation 9 | geometry_msgs 10 | #nav_msgs 11 | roscpp 12 | std_msgs 13 | genmsg 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 36 | ## pulled in transitively but can be declared for certainty nonetheless: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | add_service_files(DIRECTORY srv 63 | FILES simple_bool_service_message.srv 64 | simple_int_service_message.srv 65 | simple_float_service_message.srv 66 | arm_nav_service_message.srv 67 | IM_node_service_message.srv 68 | ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | geometry_msgs 82 | #nav_msgs 83 | ) 84 | 85 | ################################### 86 | ## catkin specific configuration ## 87 | ################################### 88 | ## The catkin_package macro generates cmake config files for your package 89 | ## Declare things to be passed to dependent projects 90 | ## INCLUDE_DIRS: uncomment this if you package contains header files 91 | ## LIBRARIES: libraries you create in this project that dependent projects also need 92 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 93 | ## DEPENDS: system dependencies of this project that dependent projects also need 94 | catkin_package( 95 | CATKIN_DEPENDS message_runtime 96 | # INCLUDE_DIRS include 97 | # LIBRARIES cwru_srv 98 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp std_msgs 99 | DEPENDS system_lib 100 | ) 101 | 102 | ########### 103 | ## Build ## 104 | ########### 105 | 106 | ## Specify additional locations of header files 107 | ## Your package locations should be listed before other locations 108 | # include_directories(include) 109 | include_directories( 110 | include ${catkin_INCLUDE_DIRS} 111 | ) 112 | 113 | ## Declare a cpp library 114 | # add_library(cwru_srv 115 | # src/${PROJECT_NAME}/cwru_srv.cpp 116 | # ) 117 | 118 | ## Declare a cpp executable 119 | add_executable(cwru_srv_trivial_node src/example_ros_service.cpp) 120 | 121 | ## Add cmake target dependencies of the executable/library 122 | ## as an example, message headers may need to be generated before nodes 123 | add_dependencies(cwru_srv_trivial_node cwru_srv_generate_messages_cpp) 124 | 125 | ## Specify libraries to link a library or executable target against 126 | target_link_libraries(cwru_srv_trivial_node 127 | ${catkin_LIBRARIES} ) 128 | 129 | ############# 130 | ## Install ## 131 | ############# 132 | 133 | # all install targets should use catkin DESTINATION variables 134 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 135 | 136 | ## Mark executable scripts (Python etc.) for installation 137 | ## in contrast to setup.py, you can choose the destination 138 | # install(PROGRAMS 139 | # scripts/my_python_script 140 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 141 | # ) 142 | 143 | ## Mark executables and/or libraries for installation 144 | # install(TARGETS cwru_srv cwru_srv_node 145 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 146 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 147 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 148 | # ) 149 | 150 | ## Mark cpp header files for installation 151 | # install(DIRECTORY include/${PROJECT_NAME}/ 152 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 153 | # FILES_MATCHING PATTERN "*.h" 154 | # PATTERN ".svn" EXCLUDE 155 | # ) 156 | 157 | ## Mark other files for installation (e.g. launch and bag files, etc.) 158 | # install(FILES 159 | # # myfile1 160 | # # myfile2 161 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 162 | # ) 163 | 164 | ############# 165 | ## Testing ## 166 | ############# 167 | 168 | ## Add gtest based cpp test target and link libraries 169 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_cwru_srv.cpp) 170 | # if(TARGET ${PROJECT_NAME}-test) 171 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 172 | # endif() 173 | 174 | ## Add folders to be run by python nosetests 175 | # catkin_add_nosetests(test) 176 | -------------------------------------------------------------------------------- /example_action_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(example_action_server) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | # example boost usage 6 | find_package(Boost REQUIRED COMPONENTS system thread) 7 | catkin_simple() 8 | 9 | # Executables 10 | cs_add_executable(example_action_server src/example_action_server.cpp) 11 | cs_add_executable(example_action_client src/example_action_client.cpp) 12 | 13 | cs_install() 14 | cs_export() 15 | 16 | -------------------------------------------------------------------------------- /example_action_server/README.md: -------------------------------------------------------------------------------- 1 | # example_action_server 2 | 3 | This example illustrates how to design an action server and an action client pair. 4 | 5 | The action server "example_action_server" implements a "simple" action server. This example is documented in the document "Introduction to action servers and clients: designing your own action server." 6 | 7 | The complementary "example_action_client" can work with any of the action servers. 8 | 9 | 10 | ## Example usage 11 | Start up roscore (or gazebo). Execute: 12 | `rosrun example_action_server example_action_server` to start the server, and: 13 | `rosrun example_action_server example_action_client` to start the client. 14 | 15 | You can monitor the communications with: 16 | `rostopic echo example_action/goal` 17 | and 18 | `rostopic echo example_action/result` 19 | 20 | 21 | ## Running tests/demos 22 | Start the server and the client, in either order (though client will time out if you wait too long to start the server). The display output should 23 | show that the client and server agree on how many requests have been serviced. However, if you halt the client and restart it, the client service 24 | count will be out of sync with the server's service count. For this demo, this mis-match causes the server to halt (for debug purposes only). 25 | -------------------------------------------------------------------------------- /example_action_server/action/demo.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | #the lines with the hash signs are merely comments 3 | #goal, result and feedback are defined by this fixed order, and separated by 3 hyphens 4 | int32 input 5 | --- 6 | #result definition 7 | int32 output 8 | int32 goal_stamp 9 | --- 10 | #feedback 11 | int32 fdbk 12 | -------------------------------------------------------------------------------- /example_action_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | example_action_server 4 | 0.0.0 5 | The example_action_server package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | catkin_simple 43 | roscpp 44 | std_msgs 45 | 46 | simple_action_client 47 | message_generation 48 | actionlib 49 | 50 | 51 | roscpp 52 | std_msgs 53 | actionlib 54 | 55 | 56 | simple_action_client 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /example_action_server/src/example_action_client.cpp: -------------------------------------------------------------------------------- 1 | // example_action_client: 2 | // wsn, October, 2014 3 | 4 | #include 5 | #include 6 | 7 | //this #include refers to the new "action" message defined for this package 8 | // the action message can be found in: .../example_action_server/action/demo.action 9 | // automated header generation creates multiple headers for message I/O 10 | // these are referred to by the root name (demo) and appended name (Action) 11 | // If you write a new client of the server in this package, you will need to include example_action_server in your package.xml, 12 | // and include the header file below 13 | #include 14 | 15 | 16 | // This function will be called once when the goal completes 17 | // this is optional, but it is a convenient way to get access to the "result" message sent by the server 18 | void doneCb(const actionlib::SimpleClientGoalState& state, 19 | const example_action_server::demoResultConstPtr& result) { 20 | ROS_INFO(" doneCb: server responded with state [%s]", state.toString().c_str()); 21 | int diff = result->output - result->goal_stamp; 22 | ROS_INFO("got result output = %d; goal_stamp = %d; diff = %d",result->output,result->goal_stamp,diff); 23 | } 24 | 25 | int main(int argc, char** argv) { 26 | ros::init(argc, argv, "demo_action_client_node"); // name this node 27 | int g_count = 0; 28 | // here is a "goal" object compatible with the server, as defined in example_action_server/action 29 | example_action_server::demoGoal goal; 30 | 31 | // use the name of our server, which is: example_action (named in example_action_server.cpp) 32 | // the "true" argument says that we want our new client to run as a separate thread (a good idea) 33 | actionlib::SimpleActionClient action_client("example_action", true); 34 | 35 | // attempt to connect to the server: 36 | ROS_INFO("waiting for server: "); 37 | bool server_exists = action_client.waitForServer(ros::Duration(5.0)); // wait for up to 5 seconds 38 | // something odd in above: does not seem to wait for 5 seconds, but returns rapidly if server not running 39 | //bool server_exists = action_client.waitForServer(); //wait forever 40 | 41 | if (!server_exists) { 42 | ROS_WARN("could not connect to server; halting"); 43 | return 0; // bail out; optionally, could print a warning message and retry 44 | } 45 | 46 | 47 | ROS_INFO("connected to action server"); // if here, then we connected to the server; 48 | 49 | while(true) { 50 | // stuff a goal message: 51 | g_count++; 52 | goal.input = g_count; // this merely sequentially numbers the goals sent 53 | //action_client.sendGoal(goal); // simple example--send goal, but do not specify callbacks 54 | action_client.sendGoal(goal,&doneCb); // we could also name additional callback functions here, if desired 55 | // action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //e.g., like this 56 | 57 | bool finished_before_timeout = action_client.waitForResult(ros::Duration(5.0)); 58 | //bool finished_before_timeout = action_client.waitForResult(); // wait forever... 59 | if (!finished_before_timeout) { 60 | ROS_WARN("giving up waiting on result for goal number %d",g_count); 61 | return 0; 62 | } 63 | else { 64 | //if here, then server returned a result to us 65 | } 66 | 67 | } 68 | 69 | return 0; 70 | } 71 | 72 | -------------------------------------------------------------------------------- /example_action_server/src/example_action_server.cpp: -------------------------------------------------------------------------------- 1 | // example_action_server: a simple action server 2 | // this version does not depend on actionlib_servers hku package 3 | // wsn, October, 2014 4 | 5 | #include 6 | #include 7 | //the following #include refers to the "action" message defined for this package 8 | // The action message can be found in: .../example_action_server/action/demo.action 9 | // Automated header generation creates multiple headers for message I/O 10 | // These are referred to by the root name (demo) and appended name (Action) 11 | #include 12 | 13 | int g_count = 0; 14 | bool g_count_failure = false; 15 | 16 | class ExampleActionServer { 17 | private: 18 | 19 | ros::NodeHandle nh_; // we'll need a node handle; get one upon instantiation 20 | 21 | // this class will own a "SimpleActionServer" called "as_". 22 | // it will communicate using messages defined in example_action_server/action/demo.action 23 | // the type "demoAction" is auto-generated from our name "demo" and generic name "Action" 24 | actionlib::SimpleActionServer as_; 25 | 26 | // here are some message types to communicate with our client(s) 27 | example_action_server::demoGoal goal_; // goal message, received from client 28 | example_action_server::demoResult result_; // put results here, to be sent back to the client when done w/ goal 29 | example_action_server::demoFeedback feedback_; // not used in this example; 30 | // would need to use: as_.publishFeedback(feedback_); to send incremental feedback to the client 31 | 32 | 33 | 34 | public: 35 | ExampleActionServer(); //define the body of the constructor outside of class definition 36 | 37 | ~ExampleActionServer(void) { 38 | } 39 | // Action Interface 40 | void executeCB(const actionlib::SimpleActionServer::GoalConstPtr& goal); 41 | }; 42 | 43 | //implementation of the constructor: 44 | // member initialization list describes how to initialize member as_ 45 | // member as_ will get instantiated with specified node-handle, name by which this server will be known, 46 | // a pointer to the function to be executed upon receipt of a goal. 47 | // 48 | // Syntax of naming the function to be invoked: get a pointer to the function, called executeCB, which is a member method 49 | // of our class exampleActionServer. Since this is a class method, we need to tell boost::bind that it is a class member, 50 | // using the "this" keyword. the _1 argument says that our executeCB takes one argument 51 | // the final argument "false" says don't start the server yet. (We'll do this in the constructor) 52 | 53 | ExampleActionServer::ExampleActionServer() : 54 | as_(nh_, "example_action", boost::bind(&ExampleActionServer::executeCB, this, _1),false) 55 | // in the above initialization, we name the server "example_action" 56 | // clients will need to refer to this name to connect with this server 57 | { 58 | ROS_INFO("in constructor of exampleActionServer..."); 59 | // do any other desired initializations here...specific to your implementation 60 | 61 | as_.start(); //start the server running 62 | } 63 | 64 | //executeCB implementation: this is a member method that will get registered with the action server 65 | // argument type is very long. Meaning: 66 | // actionlib is the package for action servers 67 | // SimpleActionServer is a templated class in this package (defined in the "actionlib" ROS package) 68 | // customizes the simple action server to use our own "action" message 69 | // defined in our package, "example_action_server", in the subdirectory "action", called "demo.action" 70 | // The name "demo" is prepended to other message types created automatically during compilation. 71 | // e.g., "demoAction" is auto-generated from (our) base name "demo" and generic name "Action" 72 | void ExampleActionServer::executeCB(const actionlib::SimpleActionServer::GoalConstPtr& goal) { 73 | //ROS_INFO("in executeCB"); 74 | //ROS_INFO("goal input is: %d", goal->input); 75 | //do work here: this is where your interesting code goes 76 | 77 | //.... 78 | 79 | // for illustration, populate the "result" message with two numbers: 80 | // the "input" is the message count, copied from goal->input (as sent by the client) 81 | // the "goal_stamp" is the server's count of how many goals it has serviced so far 82 | // if there is only one client, and if it is never restarted, then these two numbers SHOULD be identical... 83 | // unless some communication got dropped, indicating an error 84 | // send the result message back with the status of "success" 85 | 86 | g_count++; // keep track of total number of goals serviced since this server was started 87 | result_.output = g_count; // we'll use the member variable result_, defined in our class 88 | result_.goal_stamp = goal->input; 89 | 90 | // the class owns the action server, so we can use its member methods here 91 | 92 | // DEBUG: if client and server remain in sync, all is well--else whine and complain and quit 93 | // NOTE: this is NOT generically useful code; server should be happy to accept new clients at any time, and 94 | // no client should need to know how many goals the server has serviced to date 95 | if (g_count != goal->input) { 96 | ROS_WARN("hey--mismatch!"); 97 | ROS_INFO("g_count = %d; goal_stamp = %d", g_count, result_.goal_stamp); 98 | g_count_failure = true; //set a flag to commit suicide 99 | ROS_WARN("informing client of aborted goal"); 100 | as_.setAborted(result_); // tell the client we have given up on this goal; send the result message as well 101 | } 102 | else { 103 | as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message 104 | } 105 | } 106 | 107 | int main(int argc, char** argv) { 108 | ros::init(argc, argv, "demo_action_server_node"); // name this node 109 | 110 | ROS_INFO("instantiating the demo action server: "); 111 | 112 | ExampleActionServer as_object; // create an instance of the class "exampleActionServer" 113 | 114 | ROS_INFO("going into spin"); 115 | // from here, all the work is done in the action server, with the interesting stuff done within "executeCB()" 116 | // you will see 5 new topics under example_action: cancel, feedback, goal, result, status 117 | while (!g_count_failure) { 118 | ros::spinOnce(); //normally, can simply do: ros::spin(); 119 | // for debug, induce a halt if we ever get our client/server communications out of sync 120 | } 121 | 122 | return 0; 123 | } 124 | 125 | -------------------------------------------------------------------------------- /example_interactive_marker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(example_interactive_marker) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | # example boost usage 9 | # find_package(Boost REQUIRED COMPONENTS system thread) 10 | 11 | # C++0x support - not quite the same as final C++11! 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 13 | 14 | # Libraries 15 | # cs_add_libraries(my_lib src/my_lib.cpp) 16 | 17 | # Executables 18 | cs_add_executable(example_interactive_marker_node src/IM_6dof.cpp) 19 | cs_add_executable(example_im_6dof_svc_client src/IM_6dof_svc_client_test.cpp) 20 | # target_link_library(example my_lib) 21 | 22 | cs_install() 23 | cs_export() 24 | 25 | -------------------------------------------------------------------------------- /example_interactive_marker/README.md: -------------------------------------------------------------------------------- 1 | # interactive_marker_node 2 | 3 | Interactive-marker node. This node creates an interactive marker, visualizable in rviz. 4 | Topic name is: rt_hand_marker. In rviz, select this as an interactive marker subscribed to 5 | topic: rt_hand_marker/update. 6 | 7 | This node initializes the pose of the interactive marker, then listens for changes due to 8 | user interaction. It keeps track of current pose. 9 | 10 | Also provides a service called "/IM6DofSvc" which uses services messages of type: 11 | cwru_srv/IM_node_service_message. IM service requests contains a command mode. If mode is 12 | GET_CURRENT_MARKER_POSE, then the service response contains the current pose of the marker. 13 | 14 | If the command mode is SET_NEW_MARKER_POSE, then the poseStamped_IM_desired field of the 15 | request must also be filled in, specifying the desired marker pose. The service will then 16 | move the marker to this pose and save and return the new pose values. 17 | 18 | ## Example usage 19 | Run this node together with roscore and rviz. 20 | `rosrun rviz rviz`, and add an interactive-marker item, subscribed to rt_hand_marker/update. 21 | Set the fixed frame to "world." 22 | 23 | Start the interactive-marker node with: 24 | `rosrun interactive_marker_node interactive_marker_node` 25 | 26 | To test the available service, run: 27 | `rosrun interactive_marker_node im_6dof_svc_client_test` 28 | 29 | This gets the current marker pose, then causes the pose to move up by 0.1m. One can also move the marker interactively and still use IM node services to get or change the marker pose. 30 | 31 | 32 | -------------------------------------------------------------------------------- /example_interactive_marker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | example_interactive_marker 4 | 0.0.0 5 | The example_interactive_marker package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | catkin_simple 43 | roscpp 44 | interactive_markers 45 | visualization_markers 46 | std_msgs 47 | geometry_msgs 48 | example_srv 49 | tf 50 | roscpp 51 | interactive_markers 52 | visualization_markers 53 | std_msgs 54 | geometry_msgs 55 | example_srv 56 | tf 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /example_interactive_marker/src/IM_6dof.cpp: -------------------------------------------------------------------------------- 1 | // IM_6DOF_example.cpp 2 | // Wyatt Newman, based on ROS tutorial 4.2 on Interactive Markers 3 | // example 2 differs from example 1 only in that the marker frame_ID is set to "base_link" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | const int IM_GET_CURRENT_MARKER_POSE=0; 11 | const int IM_SET_NEW_MARKER_POSE= 1; 12 | 13 | 14 | geometry_msgs::Point g_current_point; 15 | geometry_msgs::Quaternion g_current_quaternion; 16 | ros::Time g_marker_time; 17 | //You must call ros::init() before creating the first NodeHandle 18 | 19 | interactive_markers::InteractiveMarkerServer *g_IM_server; //("rt_hand_marker"); 20 | visualization_msgs::InteractiveMarkerFeedback *g_IM_feedback; 21 | 22 | //service: return pose of marker from above globals; 23 | // depending on mode, move IM programmatically, 24 | 25 | bool IM6DofSvcCB(example_srv::IM_node_service_messageRequest& request, example_srv::IM_node_service_messageResponse& response) { 26 | //if busy, refuse new requests; 27 | 28 | // for a simple status query, handle it now; 29 | if (request.cmd_mode == IM_GET_CURRENT_MARKER_POSE) { 30 | ROS_INFO("IM6DofSvcCB: rcvd request for query--GET_CURRENT_MARKER_POSE"); 31 | response.poseStamped_IM_current.header.stamp = g_marker_time; 32 | response.poseStamped_IM_current.header.frame_id = "world"; 33 | response.poseStamped_IM_current.pose.position = g_current_point; 34 | response.poseStamped_IM_current.pose.orientation = g_current_quaternion; 35 | return true; 36 | } 37 | 38 | //command to move the marker to specified pose: 39 | if (request.cmd_mode == IM_SET_NEW_MARKER_POSE) { 40 | geometry_msgs::PoseStamped poseStamped_IM_desired; 41 | ROS_INFO("IM6DofSvcCB: rcvd request for action--SET_NEW_MARKER_POSE"); 42 | g_current_point = request.poseStamped_IM_desired.pose.position; 43 | g_current_quaternion = request.poseStamped_IM_desired.pose.orientation; 44 | g_marker_time = ros::Time::now(); 45 | poseStamped_IM_desired = request.poseStamped_IM_desired; 46 | poseStamped_IM_desired.header.stamp = g_marker_time; 47 | response.poseStamped_IM_current = poseStamped_IM_desired; 48 | //g_IM_feedback->pose = poseStamped_IM_desired.pose; 49 | 50 | response.poseStamped_IM_current.header.stamp = g_marker_time; 51 | response.poseStamped_IM_current.header.frame_id = "torso"; 52 | response.poseStamped_IM_current.pose.position = g_current_point; 53 | response.poseStamped_IM_current.pose.orientation = g_current_quaternion; 54 | g_IM_server->setPose("des_hand_pose",poseStamped_IM_desired.pose); //g_IM_feedback->marker_name,poseStamped_IM_desired.pose); 55 | g_IM_server->applyChanges(); 56 | return true; 57 | } 58 | ROS_WARN("IM6DofSvcCB: case not recognized"); 59 | return false; 60 | } 61 | 62 | 63 | void processFeedback( 64 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { 65 | //g_IM_feedback = feedback; // save as global 66 | ROS_INFO_STREAM(feedback->marker_name << " is now at " 67 | << feedback->pose.position.x << ", " << feedback->pose.position.y 68 | << ", " << feedback->pose.position.z); 69 | g_current_quaternion = feedback->pose.orientation; 70 | g_current_point =feedback->pose.position; 71 | g_marker_time = ros::Time::now(); 72 | //ROS_INFO_STREAM("reference frame is: "<header.frame_id); 73 | } 74 | 75 | int main(int argc, char** argv) { 76 | ros::init(argc, argv, "simple_marker"); // this will be the node name; 77 | ros::NodeHandle nh; //standard ros node handle 78 | // create an interactive marker server on the topic namespace simple_marker 79 | interactive_markers::InteractiveMarkerServer server("rt_hand_marker"); 80 | g_IM_server = &server; 81 | ros::ServiceServer IM_6dof_interface_service = nh.advertiseService("IM6DofSvc",&IM6DofSvcCB); 82 | // look for resulting pose messages on the topic: /rt_hand_marker/feedback, 83 | // which publishes a message of type visualization_msgs/InteractiveMarkerFeedback, which 84 | // includes a full "pose" of the marker. 85 | // Coordinates of the pose are with respect to the named frame 86 | 87 | // create an interactive marker for our server 88 | visualization_msgs::InteractiveMarker int_marker; 89 | 90 | int_marker.header.frame_id = "world"; //base_link"; ///world"; // the reference frame for pose coordinates 91 | int_marker.name = "des_hand_pose"; //name the marker 92 | int_marker.description = "Interactive Marker"; 93 | 94 | geometry_msgs::Point temp_point_start; 95 | /** specify/push-in the origin for this marker */ 96 | temp_point_start.x = 0.5; 97 | temp_point_start.y = -0.5; 98 | temp_point_start.z = 0.2; 99 | g_current_point.x =temp_point_start.x; 100 | g_current_point.y = temp_point_start.y; 101 | g_current_point.z = temp_point_start.z; 102 | 103 | /**/ 104 | // create an arrow marker; do this 3 times to create a triad (frame) 105 | visualization_msgs::Marker arrow_marker_x; //this one for the x axis 106 | geometry_msgs::Point temp_point; 107 | 108 | arrow_marker_x.type = visualization_msgs::Marker::ARROW; //ROS example was a CUBE; changed to ARROW 109 | // specify/push-in the origin point for the arrow 110 | temp_point.x = temp_point.y = temp_point.z = 0; 111 | arrow_marker_x.points.push_back(temp_point); 112 | // Specify and push in the end point for the arrow 113 | temp_point = temp_point_start; 114 | temp_point.x = 0.2; // arrow is this long in x direction 115 | temp_point.y = 0.0; 116 | temp_point.z = 0.0; 117 | arrow_marker_x.points.push_back(temp_point); 118 | 119 | // make the arrow very thin 120 | arrow_marker_x.scale.x = 0.01; 121 | arrow_marker_x.scale.y = 0.01; 122 | arrow_marker_x.scale.z = 0.01; 123 | 124 | arrow_marker_x.color.r = 1.0; // red, for the x axis 125 | arrow_marker_x.color.g = 0.0; 126 | arrow_marker_x.color.b = 0.0; 127 | arrow_marker_x.color.a = 1.0; 128 | 129 | // do this again for the y axis: 130 | visualization_msgs::Marker arrow_marker_y; 131 | arrow_marker_y.type = visualization_msgs::Marker::ARROW; 132 | // Push in the origin point for the arrow 133 | temp_point.x = temp_point.y = temp_point.z = 0; 134 | arrow_marker_y.points.push_back(temp_point); 135 | // Push in the end point for the arrow 136 | temp_point.x = 0.0; 137 | temp_point.y = 0.2; // points in the y direction 138 | temp_point.z = 0.0; 139 | arrow_marker_y.points.push_back(temp_point); 140 | 141 | arrow_marker_y.scale.x = 0.01; 142 | arrow_marker_y.scale.y = 0.01; 143 | arrow_marker_y.scale.z = 0.01; 144 | 145 | arrow_marker_y.color.r = 0.0; 146 | arrow_marker_y.color.g = 1.0; // color it green, for y axis 147 | arrow_marker_y.color.b = 0.0; 148 | arrow_marker_y.color.a = 1.0; 149 | 150 | // now the z axis 151 | visualization_msgs::Marker arrow_marker_z; 152 | arrow_marker_z.type = visualization_msgs::Marker::ARROW; //CUBE; 153 | // Push in the origin point for the arrow 154 | temp_point.x = temp_point.y = temp_point.z = 0; 155 | arrow_marker_z.points.push_back(temp_point); 156 | // Push in the end point for the arrow 157 | temp_point.x = 0.0; 158 | temp_point.y = 0.0; 159 | temp_point.z = 0.2; 160 | arrow_marker_z.points.push_back(temp_point); 161 | 162 | arrow_marker_z.scale.x = 0.01; 163 | arrow_marker_z.scale.y = 0.01; 164 | arrow_marker_z.scale.z = 0.01; 165 | 166 | arrow_marker_z.color.r = 0.0; 167 | arrow_marker_z.color.g = 0.0; 168 | arrow_marker_z.color.b = 1.0; 169 | arrow_marker_z.color.a = 1.0; 170 | /**/ 171 | // create a control that contains the markers 172 | visualization_msgs::InteractiveMarkerControl IM_control; 173 | IM_control.always_visible = true; 174 | //IM_control.markers.push_back(sphere_marker); 175 | 176 | IM_control.markers.push_back(arrow_marker_x); 177 | IM_control.markers.push_back(arrow_marker_y); 178 | IM_control.markers.push_back(arrow_marker_z); 179 | 180 | // add the control to the interactive marker 181 | int_marker.controls.push_back(IM_control); 182 | 183 | // create a control that will move the marker 184 | // this control does not contain any markers, 185 | // which will cause RViz to insert two arrows 186 | visualization_msgs::InteractiveMarkerControl translate_control_x; 187 | translate_control_x.name = "move_x"; 188 | translate_control_x.interaction_mode = 189 | visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 190 | 191 | /** Create the Z-Axis Control*/ 192 | visualization_msgs::InteractiveMarkerControl translate_control_z; 193 | translate_control_z.name = "move_z"; 194 | translate_control_z.interaction_mode = 195 | visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 196 | translate_control_z.orientation.x = 0; //point this in the y direction 197 | translate_control_z.orientation.y = 1; 198 | translate_control_z.orientation.z = 0; 199 | translate_control_z.orientation.w = 1; 200 | 201 | /** Create the Y-Axis Control*/ 202 | visualization_msgs::InteractiveMarkerControl translate_control_y; 203 | translate_control_y.name = "move_y"; 204 | translate_control_y.interaction_mode = 205 | visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 206 | translate_control_y.orientation.x = 0; //point this in the y direction 207 | translate_control_y.orientation.y = 0; 208 | translate_control_y.orientation.z = 1; 209 | translate_control_y.orientation.w = 1; 210 | 211 | // add x-rotation control 212 | /**/ 213 | visualization_msgs::InteractiveMarkerControl rotx_control; 214 | rotx_control.always_visible = true; 215 | rotx_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 216 | rotx_control.orientation.x = 1; 217 | rotx_control.orientation.y = 0; 218 | rotx_control.orientation.z = 0; 219 | rotx_control.orientation.w = 1; 220 | rotx_control.name = "rot_x"; 221 | 222 | // add z-rotation control 223 | visualization_msgs::InteractiveMarkerControl rotz_control; 224 | rotz_control.always_visible = true; 225 | rotz_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 226 | rotz_control.orientation.x = 0; 227 | rotz_control.orientation.y = 1; 228 | rotz_control.orientation.z = 0; 229 | rotz_control.orientation.w = 1; 230 | rotz_control.name = "rot_z"; 231 | 232 | // add y-rotation control 233 | visualization_msgs::InteractiveMarkerControl roty_control; 234 | roty_control.always_visible = true; 235 | roty_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 236 | roty_control.orientation.x = 0; 237 | roty_control.orientation.y = 0; 238 | roty_control.orientation.z = 1; 239 | roty_control.orientation.w = 1; 240 | roty_control.name = "rot_y"; 241 | /**/ 242 | // add the controls to the interactive marker 243 | int_marker.controls.push_back(translate_control_x); 244 | int_marker.controls.push_back(translate_control_y); 245 | int_marker.controls.push_back(translate_control_z); 246 | int_marker.controls.push_back(rotx_control); 247 | int_marker.controls.push_back(rotz_control); 248 | int_marker.controls.push_back(roty_control); 249 | 250 | /** Scale Down: this makes all of the arrows/disks for the user controls smaller than the default size */ 251 | int_marker.scale = 0.2; 252 | 253 | //let's pre-position the marker, else it will show up at the frame origin by default 254 | int_marker.pose.position.x = temp_point_start.x; 255 | int_marker.pose.position.y = temp_point_start.y; 256 | int_marker.pose.position.z = temp_point_start.z; 257 | 258 | // add the interactive marker to our collection & 259 | // tell the server to call processFeedback() when feedback arrives for it 260 | //server.insert(int_marker, &processFeedback); 261 | g_IM_server->insert(int_marker, &processFeedback); 262 | // 'commit' changes and send to all clients 263 | //server.applyChanges(); 264 | g_IM_server->applyChanges(); 265 | 266 | 267 | // start the ROS main loop 268 | ROS_INFO("going into spin..."); 269 | ros::spin(); 270 | } 271 | 272 | 273 | -------------------------------------------------------------------------------- /example_interactive_marker/src/IM_6dof_svc_client_test.cpp: -------------------------------------------------------------------------------- 1 | // IM_6dof_svc_client_test.cpp 2 | // wsn, June 2015; use to test IM_6dof.cpp as node interactive_marker_node 3 | // send requests via service calls; 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | //#include 16 | #include 17 | const int GET_CURRENT_MARKER_POSE=0; 18 | const int SET_NEW_MARKER_POSE= 1; 19 | 20 | using namespace std; 21 | 22 | int main(int argc, char **argv) { 23 | ros::init(argc, argv, "IM_6dof_svc_client_test"); 24 | ros::NodeHandle nh; //standard ros node handle 25 | 26 | example_srv::IM_node_service_message IM_6dof_srv_msg; 27 | geometry_msgs::Pose pose; 28 | geometry_msgs::PoseStamped poseStamped; 29 | ROS_INFO("setting up a service client of rt_hand_marker"); 30 | ros::ServiceClient IM_6dof_svc_client = nh.serviceClient("IM6DofSvc"); 31 | 32 | IM_6dof_srv_msg.request.cmd_mode = GET_CURRENT_MARKER_POSE; 33 | 34 | bool status = IM_6dof_svc_client.call(IM_6dof_srv_msg); 35 | ROS_INFO("return status: %d",status); 36 | ROS_INFO("got current marker pose: x,y,z = %f, %f, %f", 37 | IM_6dof_srv_msg.response.poseStamped_IM_current.pose.position.x, 38 | IM_6dof_srv_msg.response.poseStamped_IM_current.pose.position.y, 39 | IM_6dof_srv_msg.response.poseStamped_IM_current.pose.position.z); 40 | 41 | pose = IM_6dof_srv_msg.response.poseStamped_IM_current.pose; 42 | pose.position.z += 0.1; // increment z value 43 | poseStamped.pose = pose; 44 | poseStamped.header.stamp = ros::Time::now(); 45 | IM_6dof_srv_msg.request.cmd_mode = SET_NEW_MARKER_POSE; 46 | IM_6dof_srv_msg.request.poseStamped_IM_desired = poseStamped; 47 | ROS_INFO("moving marker up 0.1m"); 48 | status = IM_6dof_svc_client.call(IM_6dof_srv_msg); 49 | ROS_INFO("return status: %d",status); 50 | ROS_INFO("got current marker pose: x,y,z = %f, %f, %f", 51 | IM_6dof_srv_msg.response.poseStamped_IM_current.pose.position.x, 52 | IM_6dof_srv_msg.response.poseStamped_IM_current.pose.position.y, 53 | IM_6dof_srv_msg.response.poseStamped_IM_current.pose.position.z); 54 | 55 | 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /example_ros_class/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(example_ros_class) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | # example boost usage 9 | # find_package(Boost REQUIRED COMPONENTS system thread) 10 | 11 | # C++0x support - not quite the same as final C++11! 12 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 13 | 14 | # Libraries 15 | # cs_add_libraries(my_lib src/my_lib.cpp) 16 | 17 | # Executables 18 | cs_add_executable(example_ros_class src/example_ros_class.cpp) 19 | # target_link_library(example my_lib) 20 | 21 | cs_install() 22 | cs_export() 23 | -------------------------------------------------------------------------------- /example_ros_class/README.md: -------------------------------------------------------------------------------- 1 | # example_ros_class 2 | 3 | This code illustrates how to use classes to make ROS nodes. 4 | 5 | The object constructor can do the initialization work, including setting up subscribers, publishers and services. 6 | 7 | Can use member variables to pass data from subscribers to other member functions 8 | 9 | ## Example usage 10 | Can try this function manually with terminal commands, e.g.: 11 | `rosrun example_ros_class example_ros_class` 12 | 13 | Then peek/poke the various I/O options from a command line (in 3 separate terminals) with: 14 | 15 | `rostopic echo exampleMinimalPubTopic` 16 | `rostopic pub -r 4 exampleMinimalSubTopic std_msgs/Float32 2.0` 17 | `rosservice call exampleMinimalService 1` 18 | 19 | 20 | -------------------------------------------------------------------------------- /example_ros_class/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | example_ros_class 4 | 0.0.0 5 | The example_ros_class package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | catkin_simple 43 | roscpp 44 | std_msgs 45 | example_srv 46 | roscpp 47 | std_msgs 48 | example_srv 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /example_ros_class/src/example_ros_class.cpp: -------------------------------------------------------------------------------- 1 | //example_ros_class.cpp: 2 | //wsn, Feb 2015 3 | //illustrates how to use classes to make ROS nodes 4 | // constructor can do the initialization work, including setting up subscribers, publishers and services 5 | // can use member variables to pass data from subscribers to other member functions 6 | 7 | // can test this function manually with terminal commands, e.g. (in separate terminals): 8 | // rosrun example_ros_class example_ros_class 9 | // rostopic echo exampleMinimalPubTopic 10 | // rostopic pub -r 4 exampleMinimalSubTopic std_msgs/Float32 2.0 11 | // rosservice call exampleMinimalService 1 12 | 13 | 14 | // this header incorporates all the necessary #include files and defines the class "ExampleRosClass" 15 | #include "example_ros_class.h" 16 | 17 | //CONSTRUCTOR: this will get called whenever an instance of this class is created 18 | // want to put all dirty work of initializations here 19 | // odd syntax: have to pass nodehandle pointer into constructor for constructor to build subscribers, etc 20 | ExampleRosClass::ExampleRosClass(ros::NodeHandle* nodehandle):nh_(*nodehandle) 21 | { // constructor 22 | ROS_INFO("in class constructor of ExampleRosClass"); 23 | initializeSubscribers(); // package up the messy work of creating subscribers; do this overhead in constructor 24 | initializePublishers(); 25 | initializeServices(); 26 | 27 | //initialize variables here, as needed 28 | val_to_remember_=0.0; 29 | 30 | // can also do tests/waits to make sure all required services, topics, etc are alive 31 | } 32 | 33 | //member helper function to set up subscribers; 34 | // note odd syntax: &ExampleRosClass::subscriberCallback is a pointer to a member function of ExampleRosClass 35 | // "this" keyword is required, to refer to the current instance of ExampleRosClass 36 | void ExampleRosClass::initializeSubscribers() 37 | { 38 | ROS_INFO("Initializing Subscribers"); 39 | minimal_subscriber_ = nh_.subscribe("exampleMinimalSubTopic", 1, &ExampleRosClass::subscriberCallback,this); 40 | // add more subscribers here, as needed 41 | } 42 | 43 | //member helper function to set up services: 44 | // similar syntax to subscriber, required for setting up services outside of "main()" 45 | void ExampleRosClass::initializeServices() 46 | { 47 | ROS_INFO("Initializing Services"); 48 | minimal_service_ = nh_.advertiseService("exampleMinimalService", 49 | &ExampleRosClass::serviceCallback, 50 | this); 51 | // add more services here, as needed 52 | } 53 | 54 | //member helper function to set up publishers; 55 | void ExampleRosClass::initializePublishers() 56 | { 57 | ROS_INFO("Initializing Publishers"); 58 | minimal_publisher_ = nh_.advertise("exampleMinimalPubTopic", 1, true); 59 | //add more publishers, as needed 60 | // note: COULD make minimal_publisher_ a public member function, if want to use it within "main()" 61 | } 62 | 63 | 64 | 65 | // a simple callback function, used by the example subscriber. 66 | // note, though, use of member variables and access to minimal_publisher_ (which is a member method) 67 | void ExampleRosClass::subscriberCallback(const std_msgs::Float32& message_holder) { 68 | // the real work is done in this callback function 69 | // it wakes up every time a new message is published on "exampleMinimalSubTopic" 70 | 71 | val_from_subscriber_ = message_holder.data; // copy the received data into member variable, so ALL member funcs of ExampleRosClass can access it 72 | ROS_INFO("myCallback activated: received value %f",val_from_subscriber_); 73 | std_msgs::Float32 output_msg; 74 | val_to_remember_ += val_from_subscriber_; //can use a member variable to store values between calls; add incoming value each callback 75 | output_msg.data= val_to_remember_; 76 | // demo use of publisher--since publisher object is a member function 77 | minimal_publisher_.publish(output_msg); //output the square of the received value; 78 | } 79 | 80 | 81 | //member function implementation for a service callback function 82 | bool ExampleRosClass::serviceCallback(example_srv::simple_bool_service_messageRequest& request, example_srv::simple_bool_service_messageResponse& response) { 83 | ROS_INFO("service callback activated"); 84 | response.resp = true; // boring, but valid response info 85 | return true; 86 | } 87 | 88 | 89 | 90 | int main(int argc, char** argv) 91 | { 92 | // ROS set-ups: 93 | ros::init(argc, argv, "exampleRosClass"); //node name 94 | 95 | ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor 96 | 97 | ROS_INFO("main: instantiating an object of type ExampleRosClass"); 98 | ExampleRosClass exampleRosClass(&nh); //instantiate an ExampleRosClass object and pass in pointer to nodehandle for constructor to use 99 | 100 | ROS_INFO("main: going into spin; let the callbacks do all the work"); 101 | ros::spin(); 102 | return 0; 103 | } 104 | 105 | -------------------------------------------------------------------------------- /example_ros_class/src/example_ros_class.h: -------------------------------------------------------------------------------- 1 | // example_ros_class.h header file // 2 | // wsn; Feb, 2015 3 | // include this file in "example_ros_class.cpp" 4 | 5 | // here's a good trick--should always do this with header files: 6 | // create a unique mnemonic for this header file, so it will get included if needed, 7 | // but will not get included multiple times 8 | #ifndef EXAMPLE_ROS_CLASS_H_ 9 | #define EXAMPLE_ROS_CLASS_H_ 10 | 11 | //some generically useful stuff to include... 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include //ALWAYS need to include this 18 | 19 | //message types used in this example code; include more message types, as needed 20 | #include 21 | #include 22 | 23 | #include // this is a pre-defined service message, contained in shared "example_srv" package 24 | 25 | // define a class, including a constructor, member variables and member functions 26 | class ExampleRosClass 27 | { 28 | public: 29 | ExampleRosClass(ros::NodeHandle* nodehandle); //"main" will need to instantiate a ROS nodehandle, then pass it to the constructor 30 | // may choose to define public methods or public variables, if desired 31 | private: 32 | // put private member data here; "private" data will only be available to member functions of this class; 33 | ros::NodeHandle nh_; // we will need this, to pass between "main" and constructor 34 | // some objects to support subscriber, service, and publisher 35 | ros::Subscriber minimal_subscriber_; //these will be set up within the class constructor, hiding these ugly details 36 | ros::ServiceServer minimal_service_; 37 | ros::Publisher minimal_publisher_; 38 | 39 | double val_from_subscriber_; //example member variable: better than using globals; convenient way to pass data from a subscriber to other member functions 40 | double val_to_remember_; // member variables will retain their values even as callbacks come and go 41 | 42 | // member methods as well: 43 | void initializeSubscribers(); // we will define some helper methods to encapsulate the gory details of initializing subscribers, publishers and services 44 | void initializePublishers(); 45 | void initializeServices(); 46 | 47 | void subscriberCallback(const std_msgs::Float32& message_holder); //prototype for callback of example subscriber 48 | //prototype for callback for example service 49 | bool serviceCallback(example_srv::simple_bool_service_messageRequest& request, example_srv::simple_bool_service_messageResponse& response); 50 | }; // note: a class definition requires a semicolon at the end of the definition 51 | 52 | #endif // this closes the header-include trick...ALWAYS need one of these to match #ifndef 53 | -------------------------------------------------------------------------------- /example_ros_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(example_ros_msg) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | # Executables 9 | cs_add_executable(example_ros_message_publisher src/example_ros_message_publisher.cpp) 10 | 11 | cs_install() 12 | cs_export() 13 | 14 | -------------------------------------------------------------------------------- /example_ros_msg/msg/example_message.msg: -------------------------------------------------------------------------------- 1 | # here are primitives that can be used: 2 | #int8, int16, int32, int64 (plus uint*) 3 | # float32, float64 4 | # string 5 | # time, duration 6 | # other msg files 7 | # variable-length array[] and fixed-length array[C] 8 | Header header 9 | int32 demo_int 10 | float64 demo_double 11 | bool TF 12 | float64[] my_first_vector 13 | -------------------------------------------------------------------------------- /example_ros_msg/msg/example_message.msg~: -------------------------------------------------------------------------------- 1 | # here are primitives that can be used: 2 | #int8, int16, int32, int64 (plus uint*) 3 | # float32, float64 4 | # string 5 | # time, duration 6 | # other msg files 7 | # variable-length array[] and fixed-length array[C] 8 | Header header 9 | int32 demo_int 10 | float64 demo_double 11 | -------------------------------------------------------------------------------- /example_ros_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | example_ros_msg 4 | 0.0.0 5 | The example_ros_msg package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | 40 | 41 | catkin 42 | catkin_simple 43 | roscpp 44 | std_msgs 45 | catkin_simple 46 | roscpp 47 | std_msgs 48 | 49 | message_runtime 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /example_ros_msg/src/example_ros_message_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) { 6 | ros::init(argc, argv, "example_ros_message_publisher"); // name of this node 7 | ros::NodeHandle n; // two lines to create a publisher object that can talk to ROS 8 | ros::Publisher my_publisher_object = n.advertise("example_topic", 1); 9 | //"example_topic" is the name of the topic to which we will publish 10 | // the "1" argument says to use a buffer size of 1; could make larger, if expect network backups 11 | 12 | example_ros_msg::example_message my_new_message; 13 | //create a variable of type "example_msg", 14 | // as defined in this package 15 | 16 | ros::Rate naptime(1.0); //create a ros object from the ros “Rate” class; 17 | //set the sleep timer for 1Hz repetition rate (arg is in units of Hz) 18 | 19 | // put some data in the header. Do: rosmsg show std_msgs/Header 20 | // to see the definition of "Header" in std_msgs 21 | my_new_message.header.stamp = ros::Time::now(); //set the time stamp in the header; 22 | my_new_message.header.seq=0; // call this sequence number zero 23 | my_new_message.header.frame_id = "base_frame"; // would want to put true reference frame name here, if needed for coord transforms 24 | my_new_message.demo_int= 1; 25 | my_new_message.demo_double=100.0; 26 | 27 | double sqrt_arg; 28 | // do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted 29 | while (ros::ok()) 30 | { 31 | my_new_message.header.seq++; //increment the sequence counter 32 | my_new_message.header.stamp = ros::Time::now(); //update the time stamp 33 | my_new_message.demo_int*=2.0; //double the integer in this field 34 | sqrt_arg = my_new_message.demo_double; 35 | my_new_message.demo_double = sqrt(sqrt_arg); 36 | 37 | my_publisher_object.publish(my_new_message); // publish the data in new message format on topic "example_topic" 38 | //the next line will cause the loop to sleep for the balance of the desired period 39 | // to achieve the specified loop frequency 40 | naptime.sleep(); 41 | } 42 | } 43 | 44 | -------------------------------------------------------------------------------- /example_ros_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(example_ros_service) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | # Executables 9 | cs_add_executable(example_ros_service src/example_ros_service.cpp) 10 | cs_add_executable(example_ros_client src/example_ros_client.cpp) 11 | 12 | cs_install() 13 | cs_export() 14 | 15 | -------------------------------------------------------------------------------- /example_ros_service/README.md: -------------------------------------------------------------------------------- 1 | # example_ROS_service 2 | This simple example shows how to write a ROS service node. See the accompanying document, "Introduction to ROS services" 3 | 4 | ## Running tests/demos 5 | run this as: 6 | `rosrun example_ROS_service example_ROS_service` 7 | 8 | in another window, tickle it manually with (e.g.): 9 | `rosservice call lookup_by_name 'Ted'` 10 | 11 | Alternatively, run the corresponding client node with: 12 | 13 | `rosrun example_ROS_service example_ROS_client` 14 | -------------------------------------------------------------------------------- /example_ros_service/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | example_ros_service 4 | 0.0.0 5 | The example_ros_service package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | catkin_simple 44 | 45 | roscpp 46 | std_msgs 47 | roscpp 48 | std_msgs 49 | message_runtime 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /example_ros_service/src/example_ros_client.cpp: -------------------------------------------------------------------------------- 1 | //example ROS client: 2 | // first run: rosrun example_ROS_service example_ROS_service 3 | // then start this node: rosrun example_ROS_service example_ROS_client 4 | 5 | 6 | 7 | #include 8 | #include // this message type is defined in the current package 9 | #include 10 | #include 11 | using namespace std; 12 | 13 | int main(int argc, char **argv) { 14 | ros::init(argc, argv, "example_ros_client"); 15 | ros::NodeHandle n; 16 | ros::ServiceClient client = n.serviceClient("lookup_by_name"); 17 | example_ros_service::example_server_msg srv; 18 | bool found_on_list = false; 19 | string in_name; 20 | while (ros::ok()) { 21 | cout<>in_name; 24 | if (in_name.compare("x")==0) 25 | return 0; 26 | //cout<<"you entered "< 8 | #include 9 | #include 10 | #include 11 | using namespace std; 12 | 13 | bool callback(example_ros_service::example_server_msgRequest& request, example_ros_service::example_server_msgResponse& response) 14 | { 15 | ROS_INFO("callback activated"); 16 | string in_name(request.name); //let's convert this to a C++-class string, so can use member funcs 17 | //cout<<"in_name:"< 2 | 3 | example_rviz_marker 4 | 0.0.0 5 | The example_rviz_marker package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | catkin_simple 44 | roscpp 45 | std_msgs 46 | example_srv 47 | visualization_msgs 48 | catkin_simple 49 | roscpp 50 | std_msgs 51 | visualization_msgs 52 | example_srv 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /example_rviz_marker/src/example_rviz_marker.cpp: -------------------------------------------------------------------------------- 1 | #include //ALWAYS need to include this 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | using namespace std; 8 | 9 | //set these two values by service callback, make available to "main" 10 | double g_z_test = 0.0; 11 | bool g_trigger = true; 12 | 13 | //a service to prompt a new display computation. 14 | // E.g., to construct a plane at height z=1.0, trigger with: 15 | // rosservice call rviz_marker_svc 1.0 16 | bool displaySvcCB(example_srv::simple_float_service_messageRequest& request, 17 | example_srv::simple_float_service_messageResponse& response) { 18 | g_z_test = request.request_float32; 19 | ROS_INFO("example_rviz_marker: received request for height %f", g_z_test); 20 | g_trigger = true; // inform "main" a new computation is desired 21 | response.resp=true; 22 | return true; 23 | } 24 | 25 | int main(int argc, char **argv) { 26 | ros::init(argc, argv, "example_rviz_marker"); 27 | ros::NodeHandle nh; 28 | ros::Publisher vis_pub = nh.advertise("example_marker_topic", 0); 29 | visualization_msgs::Marker marker; // instantiate a marker object 30 | geometry_msgs::Point point; // points will be used to specify where the markers go 31 | 32 | //set up a service to compute marker locations on request 33 | ros::ServiceServer service = nh.advertiseService("rviz_marker_svc", displaySvcCB); 34 | 35 | 36 | marker.header.frame_id = "/world"; //generic reference frame 37 | marker.header.stamp = ros::Time(); 38 | marker.ns = "my_namespace"; 39 | marker.id = 0; 40 | // use SPHERE if you only want a single marker 41 | // use SPHERE_LIST for a group of markers 42 | marker.type = visualization_msgs::Marker::SPHERE_LIST; //SPHERE; 43 | marker.action = visualization_msgs::Marker::ADD; 44 | // if just using a single marker, specify the coordinates here, like this: 45 | 46 | //marker.pose.position.x = 0.4; 47 | //marker.pose.position.y = -0.4; 48 | //marker.pose.position.z = 0; 49 | //ROS_INFO("x,y,z = %f %f, %f",marker.pose.position.x,marker.pose.position.y, marker.pose.position.z); 50 | // otherwise, for a list of markers, put their coordinates in the "points" array, as below 51 | 52 | //whether a single marker or list of markers, need to specify marker properties 53 | // these will all be the same for SPHERE_LIST 54 | marker.pose.orientation.x = 0.0; 55 | marker.pose.orientation.y = 0.0; 56 | marker.pose.orientation.z = 0.0; 57 | marker.pose.orientation.w = 1.0; 58 | marker.scale.x = 0.02; 59 | marker.scale.y = 0.02; 60 | marker.scale.z = 0.02; 61 | marker.color.a = 1.0; 62 | marker.color.r = 1.0; 63 | marker.color.g = 0.0; 64 | marker.color.b = 0.0; 65 | 66 | double z_des; 67 | 68 | // build a wall of markers; set range and resolution 69 | double x_min = -1.0; 70 | double x_max = 1.0; 71 | double y_min = -1.0; 72 | double y_max = 1.0; 73 | double dx_des = 0.1; 74 | double dy_des = 0.1; 75 | 76 | while (ros::ok()) { 77 | if (g_trigger) { // did service call for a new computation? 78 | g_trigger = false; //reset the trigger from service 79 | z_des = g_z_test; //use z-value from service callback 80 | ROS_INFO("constructing plane of markers at height %f",z_des); 81 | marker.header.stamp = ros::Time(); 82 | marker.points.clear(); // clear out this vector 83 | 84 | for (double x_des = x_min; x_des < x_max; x_des += dx_des) { 85 | for (double y_des = y_min; y_des < y_max; y_des += dy_des) { 86 | point.x = x_des; 87 | point.y = y_des; 88 | point.z = z_des; 89 | marker.points.push_back(point); 90 | } 91 | } 92 | } 93 | ros::Duration(0.1).sleep(); 94 | //ROS_INFO("publishing..."); 95 | vis_pub.publish(marker); 96 | ros::spinOnce(); 97 | } 98 | return 0; 99 | } 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /example_srv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(example_srv) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | #message_generation 9 | geometry_msgs 10 | #nav_msgs 11 | roscpp 12 | std_msgs 13 | genmsg 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 36 | ## pulled in transitively but can be declared for certainty nonetheless: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | add_service_files(DIRECTORY srv 63 | FILES simple_bool_service_message.srv 64 | simple_int_service_message.srv 65 | simple_float_service_message.srv 66 | arm_nav_service_message.srv 67 | IM_node_service_message.srv 68 | ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | generate_messages( 79 | DEPENDENCIES 80 | std_msgs 81 | geometry_msgs 82 | #nav_msgs 83 | ) 84 | 85 | ################################### 86 | ## catkin specific configuration ## 87 | ################################### 88 | ## The catkin_package macro generates cmake config files for your package 89 | ## Declare things to be passed to dependent projects 90 | ## INCLUDE_DIRS: uncomment this if you package contains header files 91 | ## LIBRARIES: libraries you create in this project that dependent projects also need 92 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 93 | ## DEPENDS: system dependencies of this project that dependent projects also need 94 | catkin_package( 95 | CATKIN_DEPENDS message_runtime 96 | # INCLUDE_DIRS include 97 | # LIBRARIES cwru_srv 98 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp std_msgs 99 | DEPENDS system_lib 100 | ) 101 | 102 | ########### 103 | ## Build ## 104 | ########### 105 | 106 | ## Specify additional locations of header files 107 | ## Your package locations should be listed before other locations 108 | # include_directories(include) 109 | include_directories( 110 | include ${catkin_INCLUDE_DIRS} 111 | ) 112 | 113 | ## Declare a cpp library 114 | # add_library(cwru_srv 115 | # src/${PROJECT_NAME}/cwru_srv.cpp 116 | # ) 117 | 118 | ## Declare a cpp executable 119 | add_executable(example_srv_trivial_node src/example_ros_service.cpp) 120 | 121 | ## Add cmake target dependencies of the executable/library 122 | ## as an example, message headers may need to be generated before nodes 123 | add_dependencies(example_srv_trivial_node cwru_srv_generate_messages_cpp) 124 | 125 | ## Specify libraries to link a library or executable target against 126 | target_link_libraries(example_srv_trivial_node 127 | ${catkin_LIBRARIES} ) 128 | 129 | ############# 130 | ## Install ## 131 | ############# 132 | 133 | # all install targets should use catkin DESTINATION variables 134 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 135 | 136 | ## Mark executable scripts (Python etc.) for installation 137 | ## in contrast to setup.py, you can choose the destination 138 | # install(PROGRAMS 139 | # scripts/my_python_script 140 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 141 | # ) 142 | 143 | ## Mark executables and/or libraries for installation 144 | # install(TARGETS cwru_srv cwru_srv_node 145 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 146 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 147 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 148 | # ) 149 | 150 | ## Mark cpp header files for installation 151 | # install(DIRECTORY include/${PROJECT_NAME}/ 152 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 153 | # FILES_MATCHING PATTERN "*.h" 154 | # PATTERN ".svn" EXCLUDE 155 | # ) 156 | 157 | ## Mark other files for installation (e.g. launch and bag files, etc.) 158 | # install(FILES 159 | # # myfile1 160 | # # myfile2 161 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 162 | # ) 163 | 164 | ############# 165 | ## Testing ## 166 | ############# 167 | 168 | ## Add gtest based cpp test target and link libraries 169 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_cwru_srv.cpp) 170 | # if(TARGET ${PROJECT_NAME}-test) 171 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 172 | # endif() 173 | 174 | ## Add folders to be run by python nosetests 175 | # catkin_add_nosetests(test) 176 | -------------------------------------------------------------------------------- /example_srv/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | example_srv 4 | 0.0.0 5 | The example_srv package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | message_generation 44 | geometry_msgs 45 | nav_msgs 46 | roscpp 47 | std_msgs 48 | geometry_msgs 49 | nav_msgs 50 | roscpp 51 | std_msgs 52 | message_runtime 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /example_srv/src/example_ros_service.cpp: -------------------------------------------------------------------------------- 1 | //example ROS service: 2 | #include 3 | // put this here to to test that cwru_srv messages get made... 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | using namespace std; 14 | 15 | bool callback(cwru_srv::simple_bool_service_messageRequest& request, cwru_srv::simple_bool_service_messageResponse& response) 16 | { 17 | ROS_INFO("callback activated"); 18 | 19 | 20 | return true; 21 | } 22 | 23 | int main(int argc, char **argv) 24 | { 25 | ros::init(argc, argv, "example_ros_service"); 26 | ros::NodeHandle n; 27 | 28 | ros::ServiceServer service = n.advertiseService("trivial_ros_service", callback); 29 | ROS_INFO("starting trivial ros service loop"); 30 | ros::spin(); 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /example_srv/srv/IM_node_service_message.srv: -------------------------------------------------------------------------------- 1 | #This service message has a command mode (an int) and fields to accept and/or return a pose 2 | int32 cmd_mode 3 | 4 | #some requests require the client to specify a desired pose for the marker 5 | #as pose-stamped, also includes the reference frame and time stamp 6 | geometry_msgs/PoseStamped poseStamped_IM_desired 7 | 8 | --- 9 | #return the IM pose 10 | geometry_msgs/PoseStamped poseStamped_IM_current 11 | -------------------------------------------------------------------------------- /example_srv/srv/arm_nav_service_message.srv: -------------------------------------------------------------------------------- 1 | #some requests only require a command mode, e.g. request to execute a plan 2 | int32 cmd_mode 3 | 4 | #optionally, may need to specify a joint-space arm pose for start and/or finish 5 | float32[] q_vec_start 6 | float32[] q_vec_end 7 | 8 | #some requests require the client to specify start and/or goal tool poses 9 | #as pose-stamped, also includes the reference frame and time stamp 10 | geometry_msgs/PoseStamped poseStamped_start 11 | geometry_msgs/PoseStamped poseStamped_goal 12 | 13 | #if service has already computed a valid plan, the plan id will be made available to the client; 14 | #subsequently, the client can request that a named (plan_id) plan be executed 15 | int32 plan_id 16 | 17 | #this vector can be useful for specifying desired cartesian displacements 18 | float32[] delta_p 19 | 20 | --- 21 | #some responses require only true/false (e.g., good path/ no good path) 22 | bool bool_resp 23 | 24 | #more explanation may be required--as interpretable by return codes (integers) 25 | int32 rtn_code 26 | 27 | #if a plan has been computed, the server has this in memory, associated with a plan_id 28 | #client may refer to this plan_id for further examination or execution 29 | int32 plan_id 30 | 31 | #optionally, return joint-space arm pose(s) for start and/or finish, per plan 32 | float32[] q_vec_start 33 | float32[] q_vec_end 34 | -------------------------------------------------------------------------------- /example_srv/srv/path_service_message.srv: -------------------------------------------------------------------------------- 1 | nav_msgs/Path path 2 | --- 3 | bool resp 4 | -------------------------------------------------------------------------------- /example_srv/srv/simple_bool_service_message.srv: -------------------------------------------------------------------------------- 1 | bool req 2 | --- 3 | bool resp 4 | -------------------------------------------------------------------------------- /example_srv/srv/simple_float_service_message.srv: -------------------------------------------------------------------------------- 1 | float32 request_float32 2 | --- 3 | bool resp 4 | -------------------------------------------------------------------------------- /example_srv/srv/simple_int_service_message.srv: -------------------------------------------------------------------------------- 1 | int32 req 2 | --- 3 | bool resp 4 | -------------------------------------------------------------------------------- /minimal_joint_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(minimal_joint_controller) 3 | find_package(catkin_simple REQUIRED) 4 | 5 | catkin_simple() 6 | 7 | # Executables 8 | cs_add_executable(minimal_joint_controller src/minimal_joint_controller.cpp) 9 | 10 | cs_install() 11 | cs_export() 12 | -------------------------------------------------------------------------------- /minimal_joint_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | minimal_joint_controller 4 | 0.0.0 5 | The minimal_joint_controller package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | catkin_simple 44 | gazebo_msgs 45 | sensor_msgs 46 | roscpp 47 | std_msgs 48 | catkin_simple 49 | gazebo_msgs 50 | sensor_msgs 51 | roscpp 52 | std_msgs 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /minimal_joint_controller/src/minimal_joint_controller.cpp: -------------------------------------------------------------------------------- 1 | #include //ALWAYS need to include this 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | //a simple saturation function; provide saturation threshold, sat_val, and arg to be saturated, val 12 | double sat(double val, double sat_val) { 13 | if (val>sat_val) 14 | return (sat_val); 15 | if (val< -sat_val) 16 | return (-sat_val); 17 | return val; 18 | 19 | } 20 | 21 | double g_pos_cmd=0.0; //position command input-- global var 22 | void posCmdCB(const std_msgs::Float64& pos_cmd_msg) 23 | { 24 | ROS_INFO("received value of pos_cmd is: %f",pos_cmd_msg.data); 25 | g_pos_cmd = pos_cmd_msg.data; 26 | } 27 | 28 | 29 | 30 | int main(int argc, char **argv) { 31 | ros::init(argc, argv, "example_rviz_marker"); 32 | ros::NodeHandle nh; 33 | ros::Duration half_sec(0.5); 34 | 35 | // make sure service is available before attempting to proceed, else node will crash 36 | bool service_ready = false; 37 | while (!service_ready) { 38 | service_ready = ros::service::exists("/gazebo/apply_joint_effort",true); 39 | ROS_INFO("waiting for apply_joint_effort service"); 40 | half_sec.sleep(); 41 | } 42 | ROS_INFO("apply_joint_effort service exists"); 43 | 44 | ros::ServiceClient set_trq_client = 45 | nh.serviceClient("/gazebo/apply_joint_effort"); 46 | 47 | service_ready = false; 48 | while (!service_ready) { 49 | service_ready = ros::service::exists("/gazebo/get_joint_properties",true); 50 | ROS_INFO("waiting for /gazebo/get_joint_properties service"); 51 | half_sec.sleep(); 52 | } 53 | ROS_INFO("/gazebo/get_joint_properties service exists"); 54 | 55 | ros::ServiceClient get_jnt_state_client = 56 | nh.serviceClient("/gazebo/get_joint_properties"); 57 | 58 | gazebo_msgs::ApplyJointEffort effort_cmd_srv_msg; 59 | gazebo_msgs::GetJointProperties get_joint_state_srv_msg; 60 | 61 | ros::Publisher trq_publisher = nh.advertise("jnt_trq", 1); 62 | ros::Publisher vel_publisher = nh.advertise("jnt_vel", 1); 63 | ros::Publisher pos_publisher = nh.advertise("jnt_pos", 1); 64 | ros::Publisher joint_state_publisher = nh.advertise("joint_states", 1); 65 | 66 | ros::Subscriber pos_cmd_subscriber = nh.subscribe("pos_cmd",1,posCmdCB); 67 | 68 | std_msgs::Float64 trq_msg; 69 | std_msgs::Float64 q1_msg,q1dot_msg; 70 | sensor_msgs::JointState joint_state_msg; 71 | 72 | double q1, q1dot; 73 | double dt = 0.01; 74 | ros::Duration duration(dt); 75 | ros::Rate rate_timer(1/dt); 76 | 77 | effort_cmd_srv_msg.request.joint_name = "joint1"; 78 | effort_cmd_srv_msg.request.effort = 0.0; 79 | effort_cmd_srv_msg.request.duration= duration; 80 | 81 | get_joint_state_srv_msg.request.joint_name = "joint1"; 82 | //double q1_des = 1.0; 83 | double q1_err; 84 | double Kp = 10.0; 85 | double Kv = 3; 86 | double trq_cmd; 87 | 88 | // set up the joint_state_msg fields to define a single joint, 89 | // called joint1, and initial position and vel values of 0 90 | joint_state_msg.header.stamp = ros::Time::now(); 91 | joint_state_msg.name.push_back("joint1"); 92 | joint_state_msg.position.push_back(0.0); 93 | joint_state_msg.velocity.push_back(0.0); 94 | while(ros::ok()) { 95 | get_jnt_state_client.call(get_joint_state_srv_msg); 96 | q1 = get_joint_state_srv_msg.response.position[0]; 97 | q1_msg.data = q1; 98 | pos_publisher.publish(q1_msg); 99 | 100 | q1dot = get_joint_state_srv_msg.response.rate[0]; 101 | q1dot_msg.data = q1dot; 102 | vel_publisher.publish(q1dot_msg); 103 | 104 | joint_state_msg.header.stamp = ros::Time::now(); 105 | joint_state_msg.position[0] = q1; 106 | joint_state_msg.velocity[0] = q1dot; 107 | 108 | joint_state_publisher.publish(joint_state_msg); 109 | 110 | //ROS_INFO("q1 = %f; q1dot = %f",q1,q1dot); 111 | //watch for periodicity 112 | q1_err= g_pos_cmd-q1; 113 | if (q1_err>M_PI) { 114 | q1_err -= 2*M_PI; 115 | } 116 | if (q1_err< -M_PI) { 117 | q1_err += 2*M_PI; 118 | } 119 | 120 | trq_cmd = Kp*(q1_err)-Kv*q1dot; 121 | //trq_cmd = sat(trq_cmd, 10.0); //saturate at 1 N-m 122 | trq_msg.data = trq_cmd; 123 | trq_publisher.publish(trq_msg); 124 | // send torque command to Gazebo 125 | effort_cmd_srv_msg.request.effort = trq_cmd; 126 | set_trq_client.call(effort_cmd_srv_msg); 127 | //make sure service call was successful 128 | bool result = effort_cmd_srv_msg.response.success; 129 | if (!result) 130 | ROS_WARN("service call to apply_joint_effort failed!"); 131 | ros::spinOnce(); 132 | rate_timer.sleep(); 133 | } 134 | } 135 | -------------------------------------------------------------------------------- /minimal_nodes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(minimal_nodes) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | std_msgs 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 31 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 32 | ## pulled in transitively but can be declared for certainty nonetheless: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs 70 | # ) 71 | 72 | ################################### 73 | ## catkin specific configuration ## 74 | ################################### 75 | ## The catkin_package macro generates cmake config files for your package 76 | ## Declare things to be passed to dependent projects 77 | ## INCLUDE_DIRS: uncomment this if you package contains header files 78 | ## LIBRARIES: libraries you create in this project that dependent projects also need 79 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 80 | ## DEPENDS: system dependencies of this project that dependent projects also need 81 | catkin_package( 82 | # INCLUDE_DIRS include 83 | # LIBRARIES minimal_nodesmal_publish 84 | # CATKIN_DEPENDS roscpp std_msgs 85 | # DEPENDS system_lib 86 | ) 87 | 88 | ########### 89 | ## Build ## 90 | ########### 91 | 92 | ## Specify additional locations of header files 93 | ## Your package locations should be listed before other locations 94 | # include_directories(include) 95 | include_directories( 96 | ${catkin_INCLUDE_DIRS} 97 | ) 98 | 99 | ## Declare a cpp library 100 | # add_library(minimal_nodes 101 | # src/${PROJECT_NAME}/minimal_nodes.cpp 102 | # ) 103 | 104 | ## Declare a cpp executable 105 | add_executable(minimal_publisher src/minimal_publisher.cpp) 106 | add_executable(minimal_subscriber src/minimal_subscriber.cpp) 107 | add_executable(minimal_simulator src/minimal_simulator.cpp) 108 | add_executable(minimal_controller src/minimal_controller.cpp) 109 | ## Add cmake target dependencies of the executable/library 110 | ## as an example, message headers may need to be generated before nodes 111 | # add_dependencies(minimal_nodes_node minimal_nodes_generate_messages_cpp) 112 | 113 | ## Specify libraries to link a library or executable target against 114 | target_link_libraries(minimal_publisher ${catkin_LIBRARIES} ) 115 | target_link_libraries(minimal_subscriber ${catkin_LIBRARIES} ) 116 | target_link_libraries(minimal_simulator ${catkin_LIBRARIES} ) 117 | target_link_libraries(minimal_controller ${catkin_LIBRARIES} ) 118 | ############# 119 | ## Install ## 120 | ############# 121 | 122 | # all install targets should use catkin DESTINATION variables 123 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 124 | 125 | ## Mark executable scripts (Python etc.) for installation 126 | ## in contrast to setup.py, you can choose the destination 127 | # install(PROGRAMS 128 | # scripts/my_python_script 129 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 130 | # ) 131 | 132 | ## Mark executables and/or libraries for installation 133 | # install(TARGETS minimal_nodes minimal_nodes_node 134 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 135 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 136 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 137 | # ) 138 | 139 | ## Mark cpp header files for installation 140 | # install(DIRECTORY include/${PROJECT_NAME}/ 141 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 142 | # FILES_MATCHING PATTERN "*.h" 143 | # PATTERN ".svn" EXCLUDE 144 | # ) 145 | 146 | ## Mark other files for installation (e.g. launch and bag files, etc.) 147 | # install(FILES 148 | # # myfile1 149 | # # myfile2 150 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 151 | # ) 152 | 153 | ############# 154 | ## Testing ## 155 | ############# 156 | 157 | ## Add gtest based cpp test target and link libraries 158 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_minimal_nodes.cpp) 159 | # if(TARGET ${PROJECT_NAME}-test) 160 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 161 | # endif() 162 | 163 | ## Add folders to be run by python nosetests 164 | # catkin_add_nosetests(test) 165 | -------------------------------------------------------------------------------- /minimal_nodes/README.md: -------------------------------------------------------------------------------- 1 | # MINIMAL_NODES 2 | here are source examples following the notes "Intro to ROS v4", which can be found one level up in this 3 | directory under "Documents." Includes a minimal publisher, minimal subscriber, minimal simulator and 4 | minimal controller. Also contains an example launch file. 5 | -------------------------------------------------------------------------------- /minimal_nodes/catkin_notes: -------------------------------------------------------------------------------- 1 | auto-generated: 2 | find_package(catkin REQUIRED COMPONENTS 3 | roscpp 4 | std_msgs 5 | ) 6 | 7 | and 8 | include_directories( 9 | ${catkin_INCLUDE_DIRS} 10 | ) 11 | 12 | 13 | modify lines: 14 | ## Declare a cpp executable 15 | add_executable(minimal_publisher src/minimal_publisher.cpp) 16 | 17 | ## Specify libraries to link a library or executable target against 18 | target_link_libraries(minimal_publisher 19 | ${catkin_LIBRARIES} 20 | ) 21 | -------------------------------------------------------------------------------- /minimal_nodes/launch/minimal_nodes.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /minimal_nodes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | minimal_nodes 4 | 0.0.0 5 | The minimal_nodes package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | roscpp 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /minimal_nodes/src/minimal_controller.cpp: -------------------------------------------------------------------------------- 1 | // minimal_controller node: 2 | // wsn example node that both subscribes and publishes--counterpart to minimal_simulator 3 | // subscribes to "velocity" and publishes "force_cmd" 4 | // subscribes to "vel_cmd" 5 | #include 6 | #include 7 | //global variables for callback functions to populate for use in main program 8 | std_msgs::Float64 g_velocity; 9 | std_msgs::Float64 g_vel_cmd; 10 | std_msgs::Float64 g_force; // this one does not need to be global... 11 | 12 | void myCallbackVelocity(const std_msgs::Float64& message_holder) { 13 | // check for data on topic "velocity" 14 | ROS_INFO("received velocity value is: %f", message_holder.data); 15 | g_velocity.data = message_holder.data; // post the received data in a global var for access by 16 | //main prog. 17 | } 18 | 19 | void myCallbackVelCmd(const std_msgs::Float64& message_holder) { 20 | // check for data on topic "vel_cmd" 21 | ROS_INFO("received velocity command value is: %f", message_holder.data); 22 | g_vel_cmd.data = message_holder.data; // post the received data in a global var for access by 23 | //main prog. 24 | } 25 | 26 | int main(int argc, char **argv) { 27 | ros::init(argc, argv, "minimal_controller"); //name this node 28 | // when this compiled code is run, ROS will recognize it as a node called "minimal_controller" 29 | ros::NodeHandle nh; // node handle 30 | //create 2 subscribers: one for state sensing (velocity) and one for velocity commands 31 | ros::Subscriber my_subscriber_object1 = nh.subscribe("velocity", 1, myCallbackVelocity); 32 | ros::Subscriber my_subscriber_object2 = nh.subscribe("vel_cmd", 1, myCallbackVelCmd); 33 | //publish a force command computed by this controller; 34 | ros::Publisher my_publisher_object = nh.advertise("force_cmd", 1); 35 | double Kv = 1.0; // velocity feedback gain 36 | double dt_controller = 0.1; //specify 10Hz controller sample rate (pretty slow, but 37 | //illustrative) 38 | double sample_rate = 1.0 / dt_controller; // compute the corresponding update frequency 39 | ros::Rate naptime(sample_rate); // use to regulate loop rate 40 | g_velocity.data = 0.0; //initialize velocity to zero 41 | g_force.data = 0.0; // initialize force to 0; will get updated by callback 42 | g_vel_cmd.data = 0.0; // init velocity command to zero 43 | double vel_err = 0.0; // velocity error 44 | // enter the main loop: get velocity state and velocity commands 45 | // compute command force to get system velocity to match velocity command 46 | // publish this force for use by the complementary simulator 47 | while (ros::ok()) { 48 | vel_err = g_vel_cmd.data - g_velocity.data; // compute error btwn desired and actual 49 | //velocities 50 | g_force.data = Kv*vel_err; //proportional-only velocity-error feedback defines commanded 51 | //force 52 | my_publisher_object.publish(g_force); // publish the control effort computed by this 53 | //controller 54 | ROS_INFO("force command = %f", g_force.data); 55 | ros::spinOnce(); //allow data update from callback; 56 | naptime.sleep(); // wait for remainder of specified period; 57 | } 58 | return 0; // should never get here, unless roscore dies 59 | } 60 | -------------------------------------------------------------------------------- /minimal_nodes/src/minimal_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char **argv) { 5 | ros::init(argc, argv, "minimal_publisher1"); // name of this node will be "minimal_publisher1" 6 | ros::NodeHandle n; // two lines to create a publisher object that can talk to ROS 7 | ros::Publisher my_publisher_object = n.advertise("topic1", 1); 8 | //"topic1" is the name of the topic to which we will publish 9 | // the "1" argument says to use a buffer size of 1; could make larger, if expect network backups 10 | 11 | std_msgs::Float64 input_float; //create a variable of type "Float64", 12 | // as defined in: /opt/ros/indigo/share/std_msgs 13 | // any message published on a ROS topic must have a pre-defined format, 14 | // so subscribers know how to interpret the serialized data transmission 15 | 16 | ros::Rate naptime(1.0); //create a ros object from the ros “Rate” class; 17 | //set the sleep timer for 1Hz repetition rate (arg is in units of Hz) 18 | 19 | input_float.data = 0.0; 20 | 21 | // do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted 22 | while (ros::ok()) 23 | { 24 | // this loop has no sleep timer, and thus it will consume excessive CPU time 25 | // expect one core to be 100% dedicated (wastefully) to this small task 26 | input_float.data = input_float.data + 0.001; //increment by 0.001 each iteration 27 | my_publisher_object.publish(input_float); // publish the value--of type Float64-- 28 | //to the topic "topic1" 29 | //the next line will cause the loop to sleep for the balance of the desired period 30 | // to achieve the specified loop frequency 31 | naptime.sleep(); 32 | } 33 | } 34 | 35 | -------------------------------------------------------------------------------- /minimal_nodes/src/minimal_simulator.cpp: -------------------------------------------------------------------------------- 1 | // minimal_simulator node: 2 | // wsn example node that both subscribes and publishes 3 | // does trivial system simulation, F=ma, to update velocity given F specified on topic "force_cmd" 4 | // publishes velocity on topic "velocity" 5 | #include 6 | #include 7 | std_msgs::Float64 g_velocity; 8 | std_msgs::Float64 g_force; 9 | 10 | void myCallback(const std_msgs::Float64& message_holder) { 11 | // checks for messages on topic "force_cmd" 12 | ROS_INFO("received force value is: %f", message_holder.data); 13 | g_force.data = message_holder.data; // post the received data in a global var for access by 14 | // main prog. 15 | } 16 | 17 | int main(int argc, char **argv) { 18 | ros::init(argc, argv, "minimal_simulator"); //name this node 19 | // when this compiled code is run, ROS will recognize it as a node called "minimal_simulator" 20 | ros::NodeHandle nh; // node handle 21 | //create a Subscriber object and have it subscribe to the topic "force_cmd" 22 | ros::Subscriber my_subscriber_object = nh.subscribe("force_cmd", 1, myCallback); 23 | //simulate accelerations and publish the resulting velocity; 24 | ros::Publisher my_publisher_object = nh.advertise("velocity", 1); 25 | double mass = 1.0; 26 | double dt = 0.01; //10ms integration time step 27 | double sample_rate = 1.0 / dt; // compute the corresponding update frequency 28 | ros::Rate naptime(sample_rate); 29 | g_velocity.data = 0.0; //initialize velocity to zero 30 | g_force.data = 0.0; // initialize force to 0; will get updated by callback 31 | while (ros::ok()) { 32 | g_velocity.data = g_velocity.data + (g_force.data / mass) * dt; // Euler integration of 33 | //acceleration 34 | my_publisher_object.publish(g_velocity); // publish the system state (trivial--1-D) 35 | ROS_INFO("velocity = %f", g_velocity.data); 36 | ros::spinOnce(); //allow data update from callback 37 | naptime.sleep(); // wait for remainder of specified period; this loop rate is faster than 38 | // the update rate of the 10Hz controller that specifies force_cmd 39 | // however, simulator must advance each 10ms regardless 40 | } 41 | return 0; // should never get here, unless roscore dies 42 | } 43 | -------------------------------------------------------------------------------- /minimal_nodes/src/minimal_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | void myCallback(const std_msgs::Float64& message_holder) 4 | { 5 | // the real work is done in this callback function 6 | // it wakes up every time a new message is published on "topic1" 7 | // Since this function is prompted by a message event, 8 | //it does not consume CPU time polling for new data 9 | // the ROS_INFO() function is like a printf() function, except 10 | // it publishes its output to the default rosout topic, which prevents 11 | // slowing down this function for display calls, and it makes the 12 | // data available for viewing and logging purposes 13 | ROS_INFO("received value is: %f",message_holder.data); 14 | if (message_holder.data>0.5) 15 | ROS_WARN("data is too big!!"); 16 | //really could do something interesting here with the received data...but all we do is print it 17 | } 18 | 19 | int main(int argc, char **argv) 20 | { 21 | ros::init(argc,argv,"minimal_subscriber"); //name this node 22 | // when this compiled code is run, ROS will recognize it as a node called "minimal_subscriber" 23 | ros::NodeHandle n; // need this to establish communications with our new node 24 | //create a Subscriber object and have it subscribe to the topic "topic1" 25 | // the function "myCallback" will wake up whenever a new message is published to topic1 26 | // the real work is done inside the callback function 27 | 28 | ros::Subscriber my_subscriber_object= n.subscribe("topic1",1,myCallback); 29 | 30 | ros::spin(); //this is essentially a "while(1)" statement, except it 31 | // forces refreshing wakeups upon new data arrival 32 | // main program essentially hangs here, but it must stay alive to keep the callback function alive 33 | return 0; // should never get here, unless roscore dies 34 | } 35 | -------------------------------------------------------------------------------- /minimal_robot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(minimal_robot_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 28 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 29 | ## pulled in transitively but can be declared for certainty nonetheless: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################### 70 | ## catkin specific configuration ## 71 | ################################### 72 | ## The catkin_package macro generates cmake config files for your package 73 | ## Declare things to be passed to dependent projects 74 | ## INCLUDE_DIRS: uncomment this if you package contains header files 75 | ## LIBRARIES: libraries you create in this project that dependent projects also need 76 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 77 | ## DEPENDS: system dependencies of this project that dependent projects also need 78 | catkin_package( 79 | # INCLUDE_DIRS include 80 | # LIBRARIES minimal_robot_description 81 | # CATKIN_DEPENDS other_catkin_pkg 82 | # DEPENDS system_lib 83 | ) 84 | 85 | ########### 86 | ## Build ## 87 | ########### 88 | 89 | ## Specify additional locations of header files 90 | ## Your package locations should be listed before other locations 91 | # include_directories(include) 92 | 93 | ## Declare a cpp library 94 | # add_library(minimal_robot_description 95 | # src/${PROJECT_NAME}/minimal_robot_description.cpp 96 | # ) 97 | 98 | ## Declare a cpp executable 99 | # add_executable(minimal_robot_description_node src/minimal_robot_description_node.cpp) 100 | 101 | ## Add cmake target dependencies of the executable/library 102 | ## as an example, message headers may need to be generated before nodes 103 | # add_dependencies(minimal_robot_description_node minimal_robot_description_generate_messages_cpp) 104 | 105 | ## Specify libraries to link a library or executable target against 106 | # target_link_libraries(minimal_robot_description_node 107 | # ${catkin_LIBRARIES} 108 | # ) 109 | 110 | ############# 111 | ## Install ## 112 | ############# 113 | 114 | # all install targets should use catkin DESTINATION variables 115 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 116 | 117 | ## Mark executable scripts (Python etc.) for installation 118 | ## in contrast to setup.py, you can choose the destination 119 | # install(PROGRAMS 120 | # scripts/my_python_script 121 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 122 | # ) 123 | 124 | ## Mark executables and/or libraries for installation 125 | # install(TARGETS minimal_robot_description minimal_robot_description_node 126 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 127 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 128 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 129 | # ) 130 | 131 | ## Mark cpp header files for installation 132 | # install(DIRECTORY include/${PROJECT_NAME}/ 133 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 134 | # FILES_MATCHING PATTERN "*.h" 135 | # PATTERN ".svn" EXCLUDE 136 | # ) 137 | 138 | ## Mark other files for installation (e.g. launch and bag files, etc.) 139 | # install(FILES 140 | # # myfile1 141 | # # myfile2 142 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 143 | # ) 144 | 145 | ############# 146 | ## Testing ## 147 | ############# 148 | 149 | ## Add gtest based cpp test target and link libraries 150 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_minimal_robot_description.cpp) 151 | # if(TARGET ${PROJECT_NAME}-test) 152 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 153 | # endif() 154 | 155 | ## Add folders to be run by python nosetests 156 | # catkin_add_nosetests(test) 157 | -------------------------------------------------------------------------------- /minimal_robot_description/minimal_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /minimal_robot_description/minimal_robot_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /minimal_robot_description/minimal_robot_description.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /minimal_robot_description/minimal_robot_visual.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /minimal_robot_description/minimal_robot_visual.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /minimal_robot_description/minimal_robot_w_sensor.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /minimal_robot_description/minimal_robot_w_sensor.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 1 111 | true 112 | 113 | 1.047 114 | 115 | 640 116 | 480 117 | R8G8B8 118 | 119 | 120 | 121 | 122 | 123 | 0.1 124 | 100 125 | 126 | 127 | 128 | true 129 | 10.0 130 | kinect 131 | kinect_pc_frame 132 | rgb/image_raw 133 | depth/image_raw 134 | depth/points 135 | rgb/camera_info 136 | depth/camera_info 137 | 0.4 138 | 0.07 139 | 0.0 140 | 0.0 141 | 0.0 142 | 0.0 143 | 0.0 144 | 0.0 145 | 0.0 146 | 0.0 147 | 0.0 148 | 149 | 150 | 151 | 152 | 153 | -------------------------------------------------------------------------------- /minimal_robot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | minimal_robot_description 4 | 0.0.0 5 | The minimal_robot_description package 6 | 7 | 8 | 9 | 10 | wyatt 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | --------------------------------------------------------------------------------