forked from lihuang3/ur5_ROS-Gazebo
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
d82bf3b
commit bc809b4
Showing
19 changed files
with
899 additions
and
32 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,27 @@ | ||
| # What version of CMake is needed? | ||
| cmake_minimum_required(VERSION 2.8.3) | ||
|
|
||
| # The name of this package. | ||
| project(ur5_notebook) | ||
|
|
||
| # Find the catkin build system, and any other packages on which we depend. | ||
| find_package(catkin REQUIRED COMPONENTS gazebo_msgs roscpp geometry_msgs turtlesim std_srvs std_msgs message_generation message_runtime) | ||
| # Declare our catkin package. | ||
|
|
||
| #add_message files | ||
| add_message_files(FILES blocks_poses.msg) | ||
|
|
||
| generate_messages(DEPENDENCIES std_msgs) | ||
|
|
||
| catkin_package() | ||
|
|
||
| # Specify locations of header files. | ||
| include_directories(include ${catkin_INCLUDE_DIRS}) | ||
|
|
||
| # Declare the executable, along with its source files. | ||
| add_executable(blocks_spawner blocks_spawner.cpp) | ||
| add_executable(blocks_poses_publisher blocks_poses_publisher.cpp) | ||
|
|
||
| # Specify libraries against which to link. | ||
| target_link_libraries(blocks_spawner ${catkin_LIBRARIES}) | ||
| target_link_libraries(blocks_poses_publisher ${catkin_LIBRARIES}) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,7 @@ | ||
| # message type to describe 3-D position of the cylinder blocks | ||
| # variable length array, length decided by topic /current_cylinder_blocks | ||
| # to be published as a topic | ||
|
|
||
| float64[] x # x coordinate in the world | ||
| float64[] y # y coordinate in the world | ||
| float64[] z # z coordinate in the world |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,135 @@ | ||
| // this node will publish the topic "cylinder_blocks_poses" | ||
| // including all current cylinder blocks, pose is 3-D position | ||
|
|
||
| // ros communication: | ||
| // subscribe to topic "/current_cylinder_blocks" | ||
| // subscribe to topic "/gazebo/model_states" | ||
| // publish the topic "/cylinder_blocks_poses" | ||
|
|
||
| #include <ros/ros.h> | ||
| #include <vector> | ||
| #include <string> | ||
| #include <std_msgs/Int8MultiArray.h> | ||
| #include <gazebo_msgs/ModelStates.h> | ||
| #include <ur5_notebook/blocks_poses.h> | ||
|
|
||
| // global variables | ||
| int g_quantity; | ||
| std::vector<int8_t> g_current_blocks; | ||
| std::vector<double> g_x; | ||
| std::vector<double> g_y; | ||
| std::vector<double> g_z; | ||
| bool g_current_callback_started = false; | ||
| bool g_poses_updated = false; // act as frequency control of publish loop | ||
|
|
||
| std::string intToString(int a) { | ||
| std::stringstream ss; | ||
| ss << a; | ||
| return ss.str(); | ||
| } | ||
|
|
||
| void currentCallback(const std_msgs::Int8MultiArray& current_blocks) { | ||
| // this topic contains information of what cylinder blocks have been spawned | ||
| if (!g_current_callback_started) { | ||
| // set first time started flag to true | ||
| g_current_callback_started = true; | ||
| ROS_INFO("current callback has been invoked first time"); | ||
| } | ||
| g_quantity = current_blocks.data.size(); | ||
| g_current_blocks.resize(g_quantity); | ||
| g_current_blocks = current_blocks.data; | ||
| } | ||
|
|
||
| void modelStatesCallback(const gazebo_msgs::ModelStates& current_model_states) { | ||
| // this callback update global values of cylinder positions | ||
| if (g_current_callback_started) { | ||
| // only update when currentCylinderCallback has been invoked the first time | ||
| // get cylinder blocks positions according to settings in g_current_cylinder_blocks | ||
| std::vector<double> box_x; | ||
| std::vector<double> box_y; | ||
| std::vector<double> box_z; | ||
| box_x.resize(g_quantity); | ||
| box_y.resize(g_quantity); | ||
| box_z.resize(g_quantity); | ||
| // find position of all current cylinders in topic message | ||
| bool poses_completed = true; | ||
| for (int i=0; i<g_quantity; i++) { | ||
| // get index of ith cylinder | ||
| std::string indexed_model_name; | ||
| if (g_current_blocks[i] == 0) { | ||
| indexed_model_name = "red_cylinder_" + intToString(i); | ||
| } | ||
| else { | ||
| indexed_model_name = "blue_cylinder_" + intToString(i); | ||
| } | ||
| int index = -1; | ||
| int model_quantity = current_model_states.name.size(); // number of models measured | ||
| for (int j=0; j<model_quantity; j++) { | ||
| if (current_model_states.name[j] == indexed_model_name) { | ||
| index = j; break; | ||
| } | ||
| } | ||
| if (index != -1) { | ||
| // this model name has been successfully indexed | ||
| box_x[i] = current_model_states.pose[index].position.x; | ||
| box_y[i] = current_model_states.pose[index].position.y; | ||
| box_z[i] = current_model_states.pose[index].position.z; | ||
| } | ||
| else { | ||
| // ROS_ERROR("fail to find model name in the model_states topic"); | ||
| // in the test run, there is chance that the last cylinder is not in the topic message | ||
| // and g_cylinder_quantity (fron spawner node) is larger than the cylinder quantity here | ||
| // because /gazebo/model_states are sampled at a high rate of about 1000Hz | ||
| // so the position data should be aborted if fail to find the last cylinder | ||
| poses_completed = false; | ||
| } | ||
| } | ||
| if (poses_completed) { | ||
| // only pass data to globals when they are completed | ||
| g_x.resize(g_quantity); | ||
| g_y.resize(g_quantity); | ||
| g_z.resize(g_quantity); | ||
| g_x = box_x; | ||
| g_y = box_y; | ||
| g_z = box_z; | ||
| if (!g_poses_updated) { | ||
| // reset flag to true, after updating global value of g_x, g_y, g_z | ||
| g_poses_updated = true; | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
| int main(int argc, char** argv) { | ||
| ros::init(argc, argv, "blocks_poses_publisher"); | ||
| ros::NodeHandle nh; | ||
|
|
||
| // initialize subscribers for "/current_cylinder_blocks" and "/gazebo/model_states" | ||
| ros::Subscriber current_subscriber = nh.subscribe("/current_blocks" | ||
| , 1, currentCallback); | ||
| ros::Subscriber model_states_subscriber = nh.subscribe("/gazebo/model_states", 1, modelStatesCallback); | ||
| // initialize publisher for "/cylinder_blocks_poses" | ||
| ros::Publisher poses_publisher | ||
| = nh.advertise<ur5_notebook::blocks_poses>("blocks_poses", 1); | ||
| ur5_notebook::blocks_poses current_poses_msg; | ||
|
|
||
| // publishing loop | ||
| while (ros::ok()) { | ||
| if (g_poses_updated) { | ||
| // only publish when cylinder positions are updated | ||
| // no need to publish repeated data | ||
| g_poses_updated = false; // set flag to false | ||
| // there is tiny possibility that g_x is not in the length of g_cylinder_quantity | ||
| int local_quantity = g_x.size(); // so get length of g_x | ||
| current_poses_msg.x.resize(local_quantity); | ||
| current_poses_msg.y.resize(local_quantity); | ||
| current_poses_msg.z.resize(local_quantity); | ||
| current_poses_msg.x = g_x; | ||
| current_poses_msg.y = g_y; | ||
| current_poses_msg.z = g_z; | ||
| poses_publisher.publish(current_poses_msg); | ||
| } | ||
| ros::spinOnce(); | ||
| } | ||
| return 0; | ||
| } |
Oops, something went wrong.