diff --git a/CMakeLists.txt b/CMakeLists.txt index ee87bdb..d6d4442 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(webots_ros) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs message_generation tf) +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs message_generation tf moveit_ros_planning_interface) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -28,7 +28,9 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag Int8Stamped.msg RadarTarget.msg RecognitionObject.msg + RecognitionObjects.msg StringStamped.msg + ContactPoint.msg ) ## Generate services in the 'srv' folder @@ -51,6 +53,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag display_image_paste.srv display_image_save.srv display_set_font.srv + field_disable_sf_tracking.srv + field_enable_sf_tracking.srv field_get_bool.srv field_get_color.srv field_get_count.srv @@ -60,7 +64,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag field_get_rotation.srv field_get_string.srv field_get_type.srv - field_get_type_name.srv + field_get_name.srv field_get_vec2f.srv field_get_vec3f.srv field_import_node.srv @@ -91,23 +95,34 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag mouse_get_state.srv node_add_force_or_torque.srv node_add_force_with_offset.srv + node_disable_contact_points_tracking.srv + node_disable_pose_tracking.srv + node_enable_contact_points_tracking.srv + node_enable_pose_tracking.srv node_get_center_of_mass.srv node_get_contact_point.srv + node_get_contact_points.srv node_get_contact_point_node.srv node_get_field.srv + node_get_field_by_index.srv node_get_id.srv node_get_number_of_contact_points.srv + node_get_number_of_fields.srv node_get_name.srv node_get_orientation.srv + node_get_pose.srv node_get_parent_node.srv node_get_position.srv node_get_static_balance.srv node_get_status.srv + node_get_string.srv + node_set_string.srv node_get_type.srv node_get_velocity.srv node_remove.srv node_reset_functions.srv node_move_viewpoint.srv + node_set_joint_position.srv node_set_visibility.srv node_set_velocity.srv pen_set_ink_color.srv @@ -232,16 +247,6 @@ target_link_libraries(panoramic_view_recorder ${catkin_LIBRARIES} ) -#instructions for pioneer3at node - -add_executable(pioneer3at src/pioneer3at.cpp) - -add_dependencies(pioneer3at webots_ros_generate_messages_cpp) - -target_link_libraries(pioneer3at - ${catkin_LIBRARIES} -) - ############# ## Install ## @@ -265,9 +270,6 @@ install(TARGETS catch_the_bird install(TARGETS panoramic_view_recorder RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(TARGETS pioneer3at - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - catkin_install_python(PROGRAMS scripts/ros_controller.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) @@ -285,3 +287,6 @@ install(DIRECTORY plugins install(DIRECTORY worlds DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/README.md b/README.md index c37185d..e63199d 100644 --- a/README.md +++ b/README.md @@ -12,34 +12,18 @@ ROS tutorial for Webots: - https://www.cyberbotics.com/doc/guide/using-ros -## Acknowledgements +## Acknowledgement - rosin_logo
Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. More information: rosin-project.eu -eu_flag This project has received funding from the European Union’s Horizon 2020 -research and innovation programme under grant agreement no. 732287. - -
- - - opendr_logo -
- -Supported by OpenDR - Open Deep Learning Toolkit for Robotics. -More information: opendr.eu - -eu_flag - -This project has received funding from the European Union’s Horizon 2020 -research and innovation programme under grant agreement no. 871449. +research and innovation programme under grant agreement no. 732287. diff --git a/config/tiago_steel.srdf b/config/tiago_steel.srdf new file mode 100644 index 0000000..91afebf --- /dev/null +++ b/config/tiago_steel.srdf @@ -0,0 +1,336 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/pioneer3at.launch b/launch/pioneer3at.launch index 903fa00..0b0f9cd 100644 --- a/launch/pioneer3at.launch +++ b/launch/pioneer3at.launch @@ -1,13 +1,32 @@ + + - - + + + pioneer_diff_drive_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: ['front left wheel', 'back left wheel'] + right_wheel: ['front right wheel', 'back right wheel'] + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + + + + - - + + + + + diff --git a/launch/tiago.launch b/launch/tiago.launch new file mode 100644 index 0000000..d016296 --- /dev/null +++ b/launch/tiago.launch @@ -0,0 +1,85 @@ + + + + + + arm_controller: + type: position_controllers/JointTrajectoryController + joints: + - arm_1_joint + - arm_2_joint + - arm_3_joint + - arm_4_joint + - arm_5_joint + - arm_6_joint + - arm_7_joint + constraints: + stopped_velocity_tolerance: 1 + joint_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + left_wheel: wheel_left_joint + right_wheel: wheel_right_joint + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + + + + + + + + + + + + + + + + + + robot_description_kinematics: + arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + move_group: + planning_plugin: ompl_interface/OMPLPlanner + request_adapters: + default_planner_request_adapters/AddTimeParameterization + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints + start_state_max_bounds_error: 0.1 + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_manage_controllers: true + controller_list: + - name: arm_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - arm_1_joint + - arm_2_joint + - arm_3_joint + - arm_4_joint + - arm_5_joint + - arm_6_joint + - arm_7_joint + planning_scene_monitor: + publish_planning_scene: true + publish_geometry_updates: true + publish_state_updates: true + publish_transforms_updates: true + + + + + + diff --git a/msg/ContactPoint.msg b/msg/ContactPoint.msg new file mode 100644 index 0000000..bea3324 --- /dev/null +++ b/msg/ContactPoint.msg @@ -0,0 +1,2 @@ +geometry_msgs/Point point +int32 node_id diff --git a/msg/RecognitionObject.msg b/msg/RecognitionObject.msg index 25906e1..1e357d8 100644 --- a/msg/RecognitionObject.msg +++ b/msg/RecognitionObject.msg @@ -1,4 +1,3 @@ -Header header geometry_msgs/Vector3 position geometry_msgs/Quaternion orientation geometry_msgs/Vector3 position_on_image diff --git a/msg/RecognitionObjects.msg b/msg/RecognitionObjects.msg new file mode 100644 index 0000000..8011f19 --- /dev/null +++ b/msg/RecognitionObjects.msg @@ -0,0 +1,2 @@ +Header header +webots_ros/RecognitionObject[] objects diff --git a/package.xml b/package.xml index 2653103..1d6b474 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ webots_ros - 4.0.1 + 4.1.0 The ROS package containing examples for interfacing ROS with the standard ROS controller of Webots http://wiki.ros.org/webots_ros @@ -11,6 +11,7 @@ Apache 2.0 + moveit_ros_planning_interface catkin rospy roscpp @@ -24,5 +25,9 @@ sensor_msgs message_runtime tf + moveit_ros_planning_interface + ros_control + ros_controllers + robot_state_publisher diff --git a/src/complete_test.cpp b/src/complete_test.cpp index cd2678b..fde39d9 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include #include #include @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include "ros/ros.h" @@ -68,16 +69,18 @@ #include #include #include +#include +#include #include #include #include #include #include +#include #include #include #include #include -#include #include #include #include @@ -95,22 +98,33 @@ #include #include #include +#include +#include +#include +#include #include #include #include +#include #include +#include #include #include #include +#include #include #include +#include #include #include #include +#include #include #include #include #include +#include +#include #include #include #include @@ -167,9 +181,12 @@ void cameraCallback(const sensor_msgs::Image::ConstPtr &values) { callbackCalled = true; } -void cameraRecognitionCallback(const webots_ros::RecognitionObject::ConstPtr &object) { - ROS_INFO("Camera recognition saw a '%s' (time: %d:%d).", object->model.c_str(), object->header.stamp.sec, - object->header.stamp.nsec); +void cameraRecognitionCallback(const webots_ros::RecognitionObjects::ConstPtr &objects) { + const int objectsCount = objects->objects.size(); + ROS_INFO("Camera recognition saw %d objects (time: %d:%d).", objectsCount, objects->header.stamp.sec, + objects->header.stamp.nsec); + for (int i = 0; i < objectsCount; i++) + ROS_INFO(" Recognition object %d: '%s'.", i, objects->objects[i].model.c_str()); callbackCalled = true; } @@ -238,6 +255,11 @@ void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) { callbackCalled = true; } +void altimeterCallback(const webots_ros::Float64Stamped::ConstPtr &value) { + ROS_INFO("Altimeter value is z=%f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; +} + void battery_sensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Battery level is %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; @@ -1022,6 +1044,58 @@ int main(int argc, char **argv) { sampling_period_accelerometer_client.shutdown(); time_step_client.call(time_step_srv); + //////////////////////////// + // ALTIMETER METHODS TEST // + //////////////////////////// + + ros::ServiceClient set_altimeter_client; + webots_ros::set_int altimeter_srv; + ros::Subscriber sub_altimeter_32; + + set_altimeter_client = n.serviceClient(model_name + "/altimeter/enable"); + + ros::ServiceClient sampling_period_altimeter_client; + webots_ros::get_int sampling_period_altimeter_srv; + sampling_period_altimeter_client = + n.serviceClient(model_name + "/altimeter/get_sampling_period"); + + altimeter_srv.request.value = 32; + if (set_altimeter_client.call(altimeter_srv) && altimeter_srv.response.success) { + ROS_INFO("Altimeter enabled."); + sub_altimeter_32 = n.subscribe(model_name + "/altimeter/value", 1, altimeterCallback); + callbackCalled = false; + while (sub_altimeter_32.getNumPublishers() == 0 && !callbackCalled) { + ros::spinOnce(); + time_step_client.call(time_step_srv); + } + } else { + if (!altimeter_srv.response.success) + ROS_ERROR("Sampling period is not valid."); + ROS_ERROR("Failed to enable altimeter."); + return 1; + } + + sub_altimeter_32.shutdown(); + + time_step_client.call(time_step_srv); + + sampling_period_altimeter_client.call(sampling_period_altimeter_srv); + ROS_INFO("Altimeter is enabled with a sampling period of %d.", sampling_period_altimeter_srv.response.value); + + time_step_client.call(time_step_srv); + + time_step_client.call(time_step_srv); + time_step_client.call(time_step_srv); + time_step_client.call(time_step_srv); + + sampling_period_altimeter_client.call(sampling_period_altimeter_srv); + ROS_INFO("Altimeter is disabled (sampling period is %d).", sampling_period_altimeter_srv.response.value); + + set_altimeter_client.shutdown(); + sampling_period_altimeter_client.shutdown(); + time_step_client.call(time_step_srv); + + ///////////////////////////////// // BATTERY SENSOR METHODS TEST // ///////////////////////////////// @@ -1854,8 +1928,7 @@ int main(int argc, char **argv) { ros::ServiceClient noise_inertial_unit_client; webots_ros::get_float noise_inertial_unit_srv; - noise_inertial_unit_client = - n.serviceClient(model_name + "/inertial_unit/get_noise"); + noise_inertial_unit_client = n.serviceClient(model_name + "/inertial_unit/get_noise"); if (noise_inertial_unit_client.call(noise_inertial_unit_srv)) ROS_INFO("Noise value is %f.", noise_inertial_unit_srv.response.value); else @@ -2337,6 +2410,26 @@ int main(int argc, char **argv) { get_max_torque_client.shutdown(); time_step_client.call(time_step_srv); + ros::ServiceClient rotational_motor_get_multiplier_client; + webots_ros::get_float get_multiplier_srv; + rotational_motor_get_multiplier_client = + n.serviceClient(model_name + "/rotational_motor/get_multiplier"); + + rotational_motor_get_multiplier_client.call(get_multiplier_srv); + ROS_INFO("Multiplier for rotational_motor is %f.", get_multiplier_srv.response.value); + + rotational_motor_get_multiplier_client.shutdown(); + time_step_client.call(time_step_srv); + + ros::ServiceClient linear_motor_get_multiplier_client; + linear_motor_get_multiplier_client = n.serviceClient(model_name + "/linear_motor/get_multiplier"); + + linear_motor_get_multiplier_client.call(get_multiplier_srv); + ROS_INFO("Multiplier for linear_motor is %f.", get_multiplier_srv.response.value); + + linear_motor_get_multiplier_client.shutdown(); + time_step_client.call(time_step_srv); + ros::ServiceClient set_motor_feedback_client; webots_ros::set_int motor_feedback_srv; ros::Subscriber sub_motor_feedback_32; @@ -2983,6 +3076,7 @@ int main(int argc, char **argv) { supervisor_node_get_position_client.shutdown(); time_step_client.call(time_step_srv); + // supervisor_node_get_orientation ros::ServiceClient supervisor_node_get_orientation_client; webots_ros::node_get_orientation supervisor_node_get_orientation_srv; supervisor_node_get_orientation_client = @@ -2998,6 +3092,49 @@ int main(int argc, char **argv) { supervisor_node_get_orientation_client.shutdown(); time_step_client.call(time_step_srv); + // supervisor_node_get_pose + ros::ServiceClient supervisor_node_get_pose_client; + webots_ros::node_get_pose supervisor_node_get_pose_srv; + supervisor_node_get_pose_client = n.serviceClient(model_name + "/supervisor/node/get_pose"); + + supervisor_node_get_pose_srv.request.from_node = from_def_node; + supervisor_node_get_pose_srv.request.node = from_def_node; + supervisor_node_get_pose_client.call(supervisor_node_get_pose_srv); + ROS_INFO("From_def get_pose rotation is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_pose_srv.response.pose.rotation.w, + supervisor_node_get_pose_srv.response.pose.rotation.x, supervisor_node_get_pose_srv.response.pose.rotation.y, + supervisor_node_get_pose_srv.response.pose.rotation.z); + ROS_INFO("From_def get_pose translation is:\nx=%f y=%f z=%f.", supervisor_node_get_pose_srv.response.pose.translation.x, + supervisor_node_get_pose_srv.response.pose.translation.y, supervisor_node_get_pose_srv.response.pose.translation.z); + + supervisor_node_get_pose_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_node_enable_pose_tracking + ros::ServiceClient supervisor_node_enable_pose_tracking_client; + webots_ros::node_enable_pose_tracking supervisor_node_enable_pose_tracking_srv; + supervisor_node_enable_pose_tracking_client = + n.serviceClient(model_name + "/supervisor/node/enable_pose_tracking"); + + supervisor_node_enable_pose_tracking_srv.request.from_node = from_def_node; + supervisor_node_enable_pose_tracking_srv.request.node = from_def_node; + supervisor_node_enable_pose_tracking_srv.request.sampling_period = 32; + supervisor_node_enable_pose_tracking_client.call(supervisor_node_enable_pose_tracking_srv); + supervisor_node_enable_pose_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_node_disable_pose_tracking + ros::ServiceClient supervisor_node_disable_pose_tracking_client; + webots_ros::node_disable_pose_tracking supervisor_node_disable_pose_tracking_srv; + supervisor_node_disable_pose_tracking_client = + n.serviceClient(model_name + "/supervisor/node/disable_pose_tracking"); + + supervisor_node_disable_pose_tracking_srv.request.from_node = from_def_node; + supervisor_node_disable_pose_tracking_srv.request.node = from_def_node; + supervisor_node_disable_pose_tracking_client.call(supervisor_node_disable_pose_tracking_srv); + supervisor_node_disable_pose_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_node_get_center_of_mass ros::ServiceClient supervisor_node_get_center_of_mass_client; webots_ros::node_get_center_of_mass supervisor_node_get_center_of_mass_srv; supervisor_node_get_center_of_mass_client = @@ -3054,6 +3191,48 @@ int main(int argc, char **argv) { supervisor_node_get_contact_point_node_client.shutdown(); time_step_client.call(time_step_srv); + // wb_supervisor_node_enable_contact_points_tracking + ros::ServiceClient supervisor_node_enable_contact_points_tracking_client; + webots_ros::node_enable_contact_points_tracking supervisor_node_enable_contact_points_tracking_srv; + supervisor_node_enable_contact_points_tracking_client = n.serviceClient( + model_name + "/supervisor/node/enable_contact_points_tracking"); + + supervisor_node_enable_contact_points_tracking_srv.request.node = from_def_node; + supervisor_node_enable_contact_points_tracking_srv.request.include_descendants = false; + supervisor_node_enable_contact_points_tracking_client.call(supervisor_node_enable_contact_points_tracking_srv); + ROS_INFO("Contact point tracking success = %d", supervisor_node_enable_contact_points_tracking_srv.response.success); + + supervisor_node_enable_contact_points_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // wb_supervisor_node_disable_contact_points_tracking + ros::ServiceClient supervisor_node_disable_contact_points_tracking_client; + webots_ros::node_disable_contact_points_tracking supervisor_node_disable_contact_points_tracking_srv; + supervisor_node_disable_contact_points_tracking_client = n.serviceClient( + model_name + "/supervisor/node/disable_contact_points_tracking"); + + supervisor_node_disable_contact_points_tracking_srv.request.node = from_def_node; + supervisor_node_disable_contact_points_tracking_srv.request.include_descendants = false; + supervisor_node_disable_contact_points_tracking_client.call(supervisor_node_disable_contact_points_tracking_srv); + ROS_INFO("Contact point tracking success = %d", supervisor_node_disable_contact_points_tracking_srv.response.success); + + supervisor_node_disable_contact_points_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // wb_supervisor_node_get_contact_points + ros::ServiceClient supervisor_node_get_contact_points_client; + webots_ros::node_get_contact_points supervisor_node_get_contact_points_srv; + supervisor_node_get_contact_points_client = + n.serviceClient(model_name + "/supervisor/node/get_contact_points"); + + supervisor_node_get_contact_points_srv.request.node = from_def_node; + supervisor_node_get_contact_points_srv.request.include_descendants = false; + supervisor_node_get_contact_points_client.call(supervisor_node_get_contact_points_srv); + ROS_INFO("From_def node got %lu contact points.", supervisor_node_get_contact_points_srv.response.contact_points.size()); + + supervisor_node_get_contact_points_client.shutdown(); + time_step_client.call(time_step_srv); + // test get_static_balance // if the node isn't a top Solid webots will throw a warning but still return true to ros ros::ServiceClient supervisor_node_get_static_balance_client; @@ -3084,6 +3263,38 @@ int main(int argc, char **argv) { supervisor_node_reset_physics_client.shutdown(); time_step_client.call(time_step_srv); + // test node_save_state + ros::ServiceClient supervisor_node_save_state_client = + n.serviceClient(model_name + "/supervisor/node/save_state"); + webots_ros::node_set_string supervisor_node_save_state_srv; + + supervisor_node_save_state_srv.request.node = from_def_node; + supervisor_node_save_state_srv.request.state_name = "dummy_state"; + if (supervisor_node_save_state_client.call(supervisor_node_save_state_srv) && + supervisor_node_save_state_srv.response.success == 1) + ROS_INFO("Node state has been saved successfully."); + else + ROS_ERROR("Failed to call service node_save_state."); + + supervisor_node_save_state_client.shutdown(); + time_step_client.call(time_step_srv); + + // test node_load_state + ros::ServiceClient supervisor_node_load_state_client = + n.serviceClient(model_name + "/supervisor/node/load_state"); + webots_ros::node_set_string supervisor_node_load_state_srv; + + supervisor_node_load_state_srv.request.node = from_def_node; + supervisor_node_load_state_srv.request.state_name = "dummy_state"; + if (supervisor_node_load_state_client.call(supervisor_node_load_state_srv) && + supervisor_node_load_state_srv.response.success == 1) + ROS_INFO("Node state has been loaded successfully."); + else + ROS_ERROR("Failed to call service node_load_state."); + + supervisor_node_load_state_client.shutdown(); + time_step_client.call(time_step_srv); + // test restart_controller ros::ServiceClient supervisor_node_restart_controller_client = n.serviceClient(model_name + "/supervisor/node/restart_controller"); @@ -3112,6 +3323,38 @@ int main(int argc, char **argv) { supervisor_node_get_field_client.shutdown(); time_step_client.call(time_step_srv); + ros::ServiceClient wb_supervisor_node_get_number_of_fields_client; + webots_ros::node_get_number_of_fields wb_supervisor_node_get_number_of_fields_srv; + wb_supervisor_node_get_number_of_fields_client = + n.serviceClient(model_name + "/supervisor/node/get_field_by_index"); + wb_supervisor_node_get_number_of_fields_srv.request.node = root_node; + wb_supervisor_node_get_number_of_fields_srv.request.proto = 0; + wb_supervisor_node_get_number_of_fields_client.call(wb_supervisor_node_get_number_of_fields_srv); + ROS_INFO("World's root Group node have %d fields.", wb_supervisor_node_get_number_of_fields_srv.response.value); + wb_supervisor_node_get_number_of_fields_client.shutdown(); + time_step_client.call(time_step_srv); + + ros::ServiceClient wb_supervisor_node_get_field_by_index_client; + webots_ros::node_get_field_by_index wb_supervisor_node_get_field_by_index_srv; + wb_supervisor_node_get_field_by_index_client = + n.serviceClient(model_name + "/supervisor/node/get_field_by_index"); + wb_supervisor_node_get_field_by_index_srv.request.node = root_node; + wb_supervisor_node_get_field_by_index_srv.request.index = 0; + wb_supervisor_node_get_field_by_index_client.call(wb_supervisor_node_get_field_by_index_srv); + ROS_INFO("World's root Group node has a single 'children' field: %d.", + wb_supervisor_node_get_field_by_index_srv.response.field == field); + wb_supervisor_node_get_field_by_index_client.shutdown(); + time_step_client.call(time_step_srv); + + ros::ServiceClient supervisor_field_get_name_client; + webots_ros::field_get_name supervisor_field_get_name_srv; + supervisor_field_get_name_client = n.serviceClient(model_name + "/supervisor/field/get_name"); + supervisor_field_get_name_srv.request.field = field; + supervisor_field_get_name_client.call(supervisor_field_get_name_srv); + ROS_INFO("World's children field has name '%s'.", supervisor_field_get_name_srv.response.name.c_str()); + supervisor_field_get_name_client.shutdown(); + time_step_client.call(time_step_srv); + ros::ServiceClient supervisor_field_get_type_client; webots_ros::field_get_type supervisor_field_get_type_srv; supervisor_field_get_type_client = n.serviceClient(model_name + "/supervisor/field/get_type"); @@ -3124,9 +3367,9 @@ int main(int argc, char **argv) { time_step_client.call(time_step_srv); ros::ServiceClient supervisor_field_get_type_name_client; - webots_ros::field_get_type_name supervisor_field_get_type_name_srv; + webots_ros::field_get_name supervisor_field_get_type_name_srv; supervisor_field_get_type_name_client = - n.serviceClient(model_name + "/supervisor/field/get_type_name"); + n.serviceClient(model_name + "/supervisor/field/get_type_name"); supervisor_field_get_type_name_srv.request.field = field; supervisor_field_get_type_name_client.call(supervisor_field_get_type_name_srv); @@ -3170,6 +3413,40 @@ int main(int argc, char **argv) { supervisor_field_set_string_client.shutdown(); time_step_client.call(time_step_srv); + // supervisor_field_enable_sf_tracking + ros::ServiceClient supervisor_field_enable_sf_tracking_client; + webots_ros::field_enable_sf_tracking supervisor_field_enable_sf_tracking_srv; + supervisor_field_enable_sf_tracking_client = + n.serviceClient(model_name + "/supervisor/field/enable_sf_tracking"); + + supervisor_field_enable_sf_tracking_srv.request.field = field; + supervisor_field_enable_sf_tracking_srv.request.sampling_period = 32; + if (supervisor_field_enable_sf_tracking_client.call(supervisor_field_enable_sf_tracking_srv) && + supervisor_field_enable_sf_tracking_srv.response.success == 1) + ROS_INFO("Field is successfully tracked."); + else + ROS_ERROR("Failed to call service field_enable_sf_tracking."); + + supervisor_field_enable_sf_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_field_disable_sf_tracking + ros::ServiceClient supervisor_field_disable_sf_tracking_client; + webots_ros::field_disable_sf_tracking supervisor_field_disable_sf_tracking_srv; + supervisor_field_disable_sf_tracking_client = + n.serviceClient(model_name + "/supervisor/field/disable_sf_tracking"); + + supervisor_field_disable_sf_tracking_srv.request.field = field; + if (supervisor_field_disable_sf_tracking_client.call(supervisor_field_disable_sf_tracking_srv) && + supervisor_field_disable_sf_tracking_srv.response.success == 1) + ROS_INFO("Field is successfully tracked."); + else + ROS_ERROR("Failed to call service field_disable_sf_tracking."); + + supervisor_field_disable_sf_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_field_get_string_client ros::ServiceClient supervisor_field_get_string_client; webots_ros::field_get_string supervisor_field_get_string_srv; supervisor_field_get_string_client = @@ -3210,6 +3487,16 @@ int main(int argc, char **argv) { } else ROS_ERROR("could not get CONE node from DEF."); + supervisor_get_from_def_srv.request.name = "HINGE_JOINT"; + supervisor_get_from_def_srv.request.proto = 0; + supervisor_get_from_def_client.call(supervisor_get_from_def_srv); + uint64_t hinge_joint_node = 0; + if (supervisor_get_from_def_srv.response.node != 0) { + ROS_INFO("Got HINGE_JOINT node from DEF: %lu.", supervisor_get_from_def_srv.response.node); + hinge_joint_node = supervisor_get_from_def_srv.response.node; + } else + ROS_ERROR("could not get HINGE_JOINT node from DEF."); + supervisor_node_get_type_name_client.shutdown(); supervisor_get_from_def_client.shutdown(); supervisor_field_get_node_client.shutdown(); @@ -3346,6 +3633,23 @@ int main(int argc, char **argv) { node_get_parent_node_client.shutdown(); time_step_client.call(time_step_srv); + // node_set_joint_position + ros::ServiceClient node_set_joint_position_client; + webots_ros::node_set_joint_position node_set_joint_position_srv; + node_set_joint_position_client = + n.serviceClient(model_name + "/supervisor/node/set_joint_position"); + node_set_joint_position_srv.request.node = hinge_joint_node; + node_set_joint_position_srv.request.position = 0.6; + node_set_joint_position_srv.request.index = 1; + node_set_joint_position_client.call(node_set_joint_position_srv); + if (node_set_joint_position_srv.response.success == 1) + ROS_INFO("Joint position set successfully."); + else + ROS_ERROR("Failed to call service node_set_joint_position."); + + node_set_joint_position_client.shutdown(); + time_step_client.call(time_step_srv); + // movie ros::ServiceClient supervisor_movie_is_ready_client; webots_ros::node_get_status supervisor_movie_is_ready_srv; @@ -3439,6 +3743,21 @@ int main(int argc, char **argv) { remove_node_client.shutdown(); time_step_client.call(time_step_srv); + // node_export_string + ros::ServiceClient node_export_string_client; + webots_ros::node_get_string node_export_string_srv; + node_export_string_client = n.serviceClient(model_name + "/supervisor/node/export_string"); + node_export_string_srv.request.node = root_node; + node_export_string_client.call(node_export_string_srv); + std::string export_string_result = node_export_string_srv.response.value; + if (!export_string_result.find("WorldInfo {") != std::string::npos) + ROS_INFO("Node exported successfully."); + else + ROS_ERROR("Failed to call service node_export_string."); + + node_export_string_client.shutdown(); + time_step_client.call(time_step_srv); + // html robot window ros::ServiceClient wwi_send_client; wwi_send_client = n.serviceClient(model_name + "/robot/wwi_send_text"); @@ -3535,4 +3854,3 @@ int main(int argc, char **argv) { printf("\nTest Completed\n"); return 0; } - diff --git a/src/panoramic_view_recorder.cpp b/src/panoramic_view_recorder.cpp index 9b3bf02..7f69439 100644 --- a/src/panoramic_view_recorder.cpp +++ b/src/panoramic_view_recorder.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/pioneer3at.cpp b/src/pioneer3at.cpp deleted file mode 100644 index caa4c35..0000000 --- a/src/pioneer3at.cpp +++ /dev/null @@ -1,334 +0,0 @@ -// Copyright 1996-2020 Cyberbotics Ltd. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * To run gmapping you should start gmapping: - * rosrun gmapping slam_gmapping scan:=/pioneer3at/Sick_LMS_291/laser_scan/layer0 _xmax:=30 _xmin:=-30 _ymax:=30 _ymin:=-30 - * _delta:=0.2 - */ - -#include -#include -#include -#include -#include -#include -#include -#include "ros/ros.h" - -#include -#include - -#define TIME_STEP 32 -#define NMOTORS 4 -#define MAX_SPEED 6.4 -#define OBSTACLE_THRESHOLD 0.1 -#define DECREASE_FACTOR 0.9 -#define BACK_SLOWDOWN 0.9 - -ros::NodeHandle *n; - -static std::vector lidarValues; - -static int controllerCount; -static std::vector controllerList; - -ros::ServiceClient timeStepClient; -webots_ros::set_int timeStepSrv; - -static const char *motorNames[NMOTORS] = {"front_left_wheel", "front_right_wheel", "back_left_wheel", "back_right_wheel"}; - -static double GPSValues[2] = {0, 0}; -static double inertialUnitValues[4] = {0, 0, 0, 0}; - -static int lms291Resolution = 0; -static int halfResolution = 0; -static double maxRange = 0.0; -static double rangeThreshold = 0.0; -static std::vector braitenbergCoefficients; -static bool areBraitenbergCoefficientsinitialized = false; - -// gaussian function -double gaussian(double x, double mu, double sigma) { - return (1.0 / (sigma * sqrt(2.0 * M_PI))) * exp(-((x - mu) * (x - mu)) / (2 * sigma * sigma)); -} - -void updateSpeed() { - // init dynamic variables - double leftObstacle = 0.0, rightObstacle = 0.0, obstacle = 0.0; - double speeds[NMOTORS]; - // apply the braitenberg coefficients on the resulted values of the lms291 - // near obstacle sensed on the left side - for (int i = 0; i < halfResolution; ++i) { - if (lidarValues[i] < rangeThreshold) // far obstacles are ignored - leftObstacle += braitenbergCoefficients[i] * (1.0 - lidarValues[i] / maxRange); - // near obstacle sensed on the right side - int j = lms291Resolution - i - 1; - if (lidarValues[j] < rangeThreshold) - rightObstacle += braitenbergCoefficients[i] * (1.0 - lidarValues[j] / maxRange); - } - // overall front obstacle - obstacle = leftObstacle + rightObstacle; - // compute the speed according to the information on - // obstacles - if (obstacle > OBSTACLE_THRESHOLD) { - const double speedFactor = (1.0 - DECREASE_FACTOR * obstacle) * MAX_SPEED / obstacle; - speeds[0] = speedFactor * leftObstacle; - speeds[1] = speedFactor * rightObstacle; - speeds[2] = BACK_SLOWDOWN * speeds[0]; - speeds[3] = BACK_SLOWDOWN * speeds[1]; - } else { - speeds[0] = MAX_SPEED; - speeds[1] = MAX_SPEED; - speeds[2] = MAX_SPEED; - speeds[3] = MAX_SPEED; - } - // set speeds - for (int i = 0; i < NMOTORS; ++i) { - ros::ServiceClient set_velocity_client; - webots_ros::set_float set_velocity_srv; - set_velocity_client = n->serviceClient(std::string("pioneer3at/") + std::string(motorNames[i]) + - std::string("/set_velocity")); - set_velocity_srv.request.value = speeds[i]; - set_velocity_client.call(set_velocity_srv); - } -} - -void broadcastTransform() { - static tf::TransformBroadcaster br; - tf::Transform transform; - - // Publish odometry transform - transform.setOrigin(tf::Vector3(GPSValues[0], GPSValues[1], 0)); - tf::Quaternion q(inertialUnitValues[0], inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[3]); - transform.setRotation(q); - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link")); - - // Publish Lidar transform (inverted to match ENU) - transform.setIdentity(); - transform.setRotation(tf::Quaternion(tf::Vector3(1, 0, 0), M_PI)); - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "pioneer3at/Sick_LMS_291")); -} - -void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) { - GPSValues[0] = values->point.x; - GPSValues[1] = values->point.y; - broadcastTransform(); -} - -void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) { - inertialUnitValues[0] = values->orientation.x; - inertialUnitValues[1] = values->orientation.y; - inertialUnitValues[2] = values->orientation.z; - inertialUnitValues[3] = values->orientation.w; - broadcastTransform(); -} - -void lidarCallback(const sensor_msgs::LaserScan::ConstPtr &scan) { - int scanSize = scan->ranges.size(); - lidarValues.resize(scanSize); - for (int i = 0; i < scanSize; ++i) - lidarValues[i] = scan->ranges[i]; - - lms291Resolution = scanSize; - halfResolution = scanSize / 2; - maxRange = scan->range_max; - rangeThreshold = maxRange / 20.0; - if (!areBraitenbergCoefficientsinitialized) { - braitenbergCoefficients.resize(lms291Resolution); - for (int i = 0; i < lms291Resolution; ++i) - braitenbergCoefficients[i] = gaussian(i, halfResolution, lms291Resolution / 5); - areBraitenbergCoefficientsinitialized = true; - } - - updateSpeed(); -} - -// catch names of the controllers availables on ROS network -void controllerNameCallback(const std_msgs::String::ConstPtr &name) { - controllerCount++; - controllerList.push_back(name->data); - ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str()); -} - -void quit(int sig) { - ROS_INFO("User stopped the 'pioneer3at' node."); - timeStepSrv.request.value = 0; - timeStepClient.call(timeStepSrv); - ros::shutdown(); - exit(0); -} - -int main(int argc, char **argv) { - std::string controllerName; - // create a node named 'pioneer3at' on ROS network - ros::init(argc, argv, "pioneer3at", ros::init_options::AnonymousName); - n = new ros::NodeHandle; - - signal(SIGINT, quit); - - // subscribe to the topic model_name to get the list of availables controllers - ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback); - while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) { - ros::spinOnce(); - ros::spinOnce(); - ros::spinOnce(); - } - ros::spinOnce(); - - timeStepClient = n->serviceClient("pioneer3at/robot/time_step"); - timeStepSrv.request.value = TIME_STEP; - - // if there is more than one controller available, it let the user choose - if (controllerCount == 1) - controllerName = controllerList[0]; - else { - int wantedController = 0; - std::cout << "Choose the # of the controller you want to use:\n"; - std::cin >> wantedController; - if (1 <= wantedController && wantedController <= controllerCount) - controllerName = controllerList[wantedController - 1]; - else { - ROS_ERROR("Invalid number for controller choice."); - return 1; - } - } - ROS_INFO("Using controller: '%s'", controllerName.c_str()); - // leave topic once it is not necessary anymore - nameSub.shutdown(); - - // init motors - for (int i = 0; i < NMOTORS; ++i) { - // position - ros::ServiceClient set_position_client; - webots_ros::set_float set_position_srv; - set_position_client = n->serviceClient(std::string("pioneer3at/") + std::string(motorNames[i]) + - std::string("/set_position")); - - set_position_srv.request.value = INFINITY; - if (set_position_client.call(set_position_srv) && set_position_srv.response.success) - ROS_INFO("Position set to INFINITY for motor %s.", motorNames[i]); - else - ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]); - - // speed - ros::ServiceClient set_velocity_client; - webots_ros::set_float set_velocity_srv; - set_velocity_client = n->serviceClient(std::string("pioneer3at/") + std::string(motorNames[i]) + - std::string("/set_velocity")); - - set_velocity_srv.request.value = 0.0; - if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1) - ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]); - else - ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]); - } - - // enable lidar - ros::ServiceClient set_lidar_client; - webots_ros::set_int lidar_srv; - ros::Subscriber sub_lidar_scan; - - set_lidar_client = n->serviceClient("pioneer3at/Sick_LMS_291/enable"); - lidar_srv.request.value = TIME_STEP; - if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) { - ROS_INFO("Lidar enabled."); - sub_lidar_scan = n->subscribe("pioneer3at/Sick_LMS_291/laser_scan/layer0", 10, lidarCallback); - ROS_INFO("Topic for lidar initialized."); - while (sub_lidar_scan.getNumPublishers() == 0) { - } - ROS_INFO("Topic for lidar scan connected."); - } else { - if (!lidar_srv.response.success) - ROS_ERROR("Sampling period is not valid."); - ROS_ERROR("Failed to enable lidar."); - return 1; - } - - // enable gps - ros::ServiceClient set_GPS_client; - webots_ros::set_int GPS_srv; - ros::Subscriber sub_GPS; - set_GPS_client = n->serviceClient("pioneer3at/gps/enable"); - GPS_srv.request.value = 32; - if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) { - sub_GPS = n->subscribe("pioneer3at/gps/values", 1, GPSCallback); - while (sub_GPS.getNumPublishers() == 0) { - } - ROS_INFO("GPS enabled."); - } else { - if (!GPS_srv.response.success) - ROS_ERROR("Sampling period is not valid."); - ROS_ERROR("Failed to enable GPS."); - return 1; - } - - // enable inertial unit - ros::ServiceClient set_inertial_unit_client; - webots_ros::set_int inertial_unit_srv; - ros::Subscriber sub_inertial_unit; - set_inertial_unit_client = n->serviceClient("pioneer3at/inertial_unit/enable"); - inertial_unit_srv.request.value = 32; - if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) { - sub_inertial_unit = n->subscribe("pioneer3at/inertial_unit/quaternion", 1, inertialUnitCallback); - while (sub_inertial_unit.getNumPublishers() == 0) { - } - ROS_INFO("Inertial unit enabled."); - } else { - if (!inertial_unit_srv.response.success) - ROS_ERROR("Sampling period is not valid."); - ROS_ERROR("Failed to enable inertial unit."); - return 1; - } - - // enable accelerometer - ros::ServiceClient set_accelerometer_client; - webots_ros::set_int accelerometer_srv; - ros::Subscriber sub_accelerometer; - set_accelerometer_client = n->serviceClient("pioneer3at/accelerometer/enable"); - accelerometer_srv.request.value = 32; - set_accelerometer_client.call(accelerometer_srv); - // enable camera - ros::ServiceClient set_camera_client; - webots_ros::set_int camera_srv; - ros::Subscriber sub_camera; - set_camera_client = n->serviceClient("pioneer3at/camera/enable"); - camera_srv.request.value = 64; - set_camera_client.call(camera_srv); - // enable gyro - ros::ServiceClient set_gyro_client; - webots_ros::set_int gyro_srv; - ros::Subscriber sub_gyro; - set_gyro_client = n->serviceClient("pioneer3at/gyro/enable"); - gyro_srv.request.value = 32; - set_gyro_client.call(gyro_srv); - - ROS_INFO("You can now start the creation of the map using 'rosrun gmapping slam_gmapping " - "scan:=/pioneer3at/Sick_LMS_291/laser_scan/layer0 _xmax:=30 _xmin:=-30 _ymax:=30 _ymin:=-30 _delta:=0.2'."); - ROS_INFO("You can now visualize the sensors output in rqt using 'rqt'."); - - // main loop - while (ros::ok()) { - if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) { - ROS_ERROR("Failed to call service time_step for next step."); - break; - } - ros::spinOnce(); - } - timeStepSrv.request.value = 0; - timeStepClient.call(timeStepSrv); - - ros::shutdown(); - return 0; -} diff --git a/src/robot_information_parser.cpp b/src/robot_information_parser.cpp index 7053891..4f803c8 100644 --- a/src/robot_information_parser.cpp +++ b/src/robot_information_parser.cpp @@ -90,10 +90,8 @@ int main(int argc, char **argv) { getTypeClient.call(getTypeSrv); if (getTypeSrv.response.value == 40) ROS_INFO("This controller is on a basic robot."); - else if (getTypeSrv.response.value == 41) - ROS_INFO("This controller is on a supervisor robot."); else - ROS_INFO("This controller is on a differential wheels robot."); + ROS_INFO("This controller is on a supervisor robot."); if (getModelClient.call(getModelSrv)) { if (!getModelSrv.response.value.empty()) diff --git a/srv/field_disable_sf_tracking.srv b/srv/field_disable_sf_tracking.srv new file mode 100644 index 0000000..c46a9cb --- /dev/null +++ b/srv/field_disable_sf_tracking.srv @@ -0,0 +1,3 @@ +uint64 field +--- +int8 success diff --git a/srv/field_enable_sf_tracking.srv b/srv/field_enable_sf_tracking.srv new file mode 100644 index 0000000..d7a45b5 --- /dev/null +++ b/srv/field_enable_sf_tracking.srv @@ -0,0 +1,4 @@ +uint64 field +uint64 sampling_period +--- +int8 success diff --git a/srv/field_get_type_name.srv b/srv/field_get_name.srv similarity index 100% rename from srv/field_get_type_name.srv rename to srv/field_get_name.srv diff --git a/srv/node_disable_contact_points_tracking.srv b/srv/node_disable_contact_points_tracking.srv new file mode 100644 index 0000000..654faaa --- /dev/null +++ b/srv/node_disable_contact_points_tracking.srv @@ -0,0 +1,4 @@ +uint64 node +bool include_descendants +--- +int8 success diff --git a/srv/node_disable_pose_tracking.srv b/srv/node_disable_pose_tracking.srv new file mode 100644 index 0000000..ac43d79 --- /dev/null +++ b/srv/node_disable_pose_tracking.srv @@ -0,0 +1,4 @@ +uint64 node +uint64 from_node +--- +int8 success diff --git a/srv/node_enable_contact_points_tracking.srv b/srv/node_enable_contact_points_tracking.srv new file mode 100644 index 0000000..00106fe --- /dev/null +++ b/srv/node_enable_contact_points_tracking.srv @@ -0,0 +1,5 @@ +uint64 node +int32 sampling_period +bool include_descendants +--- +int8 success diff --git a/srv/node_enable_pose_tracking.srv b/srv/node_enable_pose_tracking.srv new file mode 100644 index 0000000..cbf9ff9 --- /dev/null +++ b/srv/node_enable_pose_tracking.srv @@ -0,0 +1,5 @@ +uint64 node +int32 sampling_period +uint64 from_node +--- +int8 success diff --git a/srv/node_get_contact_points.srv b/srv/node_get_contact_points.srv new file mode 100644 index 0000000..f52d36b --- /dev/null +++ b/srv/node_get_contact_points.srv @@ -0,0 +1,4 @@ +uint64 node +bool include_descendants +--- +webots_ros/ContactPoint[] contact_points diff --git a/srv/node_get_field_by_index.srv b/srv/node_get_field_by_index.srv new file mode 100644 index 0000000..4a93929 --- /dev/null +++ b/srv/node_get_field_by_index.srv @@ -0,0 +1,5 @@ +uint64 node +uint32 index +bool proto +--- +uint64 field diff --git a/srv/node_get_number_of_fields.srv b/srv/node_get_number_of_fields.srv new file mode 100644 index 0000000..9533e56 --- /dev/null +++ b/srv/node_get_number_of_fields.srv @@ -0,0 +1,4 @@ +uint64 node +bool proto +--- +int32 value diff --git a/srv/node_get_pose.srv b/srv/node_get_pose.srv new file mode 100644 index 0000000..bc6d15c --- /dev/null +++ b/srv/node_get_pose.srv @@ -0,0 +1,4 @@ +uint64 node +uint64 from_node +--- +geometry_msgs/Transform pose diff --git a/srv/node_get_string.srv b/srv/node_get_string.srv new file mode 100644 index 0000000..2218a1a --- /dev/null +++ b/srv/node_get_string.srv @@ -0,0 +1,3 @@ +uint64 node +--- +string value diff --git a/srv/node_set_joint_position.srv b/srv/node_set_joint_position.srv new file mode 100644 index 0000000..282eb06 --- /dev/null +++ b/srv/node_set_joint_position.srv @@ -0,0 +1,5 @@ +uint64 node +float64 position +int32 index +--- +bool success diff --git a/srv/node_set_string.srv b/srv/node_set_string.srv new file mode 100644 index 0000000..44068dc --- /dev/null +++ b/srv/node_set_string.srv @@ -0,0 +1,4 @@ +uint64 node +string state_name +--- +int32 success diff --git a/worlds/.catch_the_bird.wbproj b/worlds/.catch_the_bird.wbproj index 61983e4..eb1d22b 100644 --- a/worlds/.catch_the_bird.wbproj +++ b/worlds/.catch_the_bird.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000190000033d0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000002ec0000025efc0200000001fb0000001400540065007800740045006400690074006f007200000000190000025e0000003f00ffffff00000003000004e20000005afc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000004e20000006900ffffff000004e20000015700000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000002060100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000002010000000201 diff --git a/worlds/.complete_test.wbproj b/worlds/.complete_test.wbproj index efddce6..45f5eb4 100644 --- a/worlds/.complete_test.wbproj +++ b/worlds/.complete_test.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000018f00000482fc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000190000033d0000000000000000fb0000002c0052006f0062006f0074003a002000660075006c006c005f00730075007000650072007600690073006f007201000000000000018f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f007701000000000000018f0000005400fffffffb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000001ed000003a2fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003a20000003c00ffffff00000003000005aa000000dafc0100000001fb0000000e0043006f006e0073006f006c00650100000195000005aa0000005400ffffff000003b7000003a200000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000170000003490100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201 diff --git a/worlds/.e-puck_line.wbproj b/worlds/.e-puck_line.wbproj index 5f9b1ce..a0361e0 100644 --- a/worlds/.e-puck_line.wbproj +++ b/worlds/.e-puck_line.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000016c0000033dfc0200000001fb00000012005300630065006e0065005400720065006500000000190000033d0000000000000000000000010000016200000309fc0200000001fb0000001400540065007800740045006400690074006f00720100000019000003090000003e00ffffff000000030000073f000000e2fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005a00ffffff000005d70000030900000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000002850100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000a80100000006010000000201 diff --git a/worlds/.keyboard_teleop.wbproj b/worlds/.keyboard_teleop.wbproj index 1289b98..2dff954 100644 --- a/worlds/.keyboard_teleop.wbproj +++ b/worlds/.keyboard_teleop.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000016c0000033dfc0200000001fb00000012005300630065006e0065005400720065006501000000190000033d000000000000000000000001000001e100000258fc0200000001fb0000001400540065007800740045006400690074006f00720100000019000002580000003e00ffffff000000030000073f00000193fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005a00ffffff000005580000025800000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000002060100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000a80100000006010000000201 diff --git a/worlds/.panoramic_view_recorder.wbproj b/worlds/.panoramic_view_recorder.wbproj index 3f4da1a..19379cb 100644 --- a/worlds/.panoramic_view_recorder.wbproj +++ b/worlds/.panoramic_view_recorder.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000012e000003a2fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003a20000003c00ffffff000000030000073f000000dafc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005400ffffff0000060b000003a200000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000006010000000201 diff --git a/worlds/.pioneer3at.wbproj b/worlds/.pioneer3at.wbproj index 725746c..86294d3 100644 --- a/worlds/.pioneer3at.wbproj +++ b/worlds/.pioneer3at.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000012e000002d9fc0200000001fb0000001400540065007800740045006400690074006f00720000000019000002d9000000a200ffffff000000030000073f000000bbfc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005400ffffff0000073f000003c100000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000011e000004db0100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000001240100000006010000000201 diff --git a/worlds/.ros_python.wbproj b/worlds/.ros_python.wbproj index 158ad59..9022d0c 100644 --- a/worlds/.ros_python.wbproj +++ b/worlds/.ros_python.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000001c900000375fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003750000008900ffffff000000030000073f00000093fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000006900ffffff000005740000037500000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000100000005210100000002010000000100 sceneTreePerspectives: 000000ff0000000100000002000002aa000000fa0100000002010000000200 diff --git a/worlds/.tiago.wbproj b/worlds/.tiago.wbproj new file mode 100644 index 0000000..b876f99 --- /dev/null +++ b/worlds/.tiago.wbproj @@ -0,0 +1,11 @@ +Webots Project File version R2021b +perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000011c0000038ffc0200000001fb0000001400540065007800740045006400690074006f007200000000170000038f0000003f00ffffff000000030000078000000087fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000034100000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff000000010000000200000100000003820100000002010000000100 +sceneTreePerspectives: 000000ff0000000100000002000000e0000000fa0100000002010000000200 +maximizedDockId: -1 +centralWidgetVisible: 1 +orthographicViewHeight: 1 +textFiles: -1 +consoles: Console:All:All +renderingDevicePerspectives: e-puck:camera;0;1;0;0 +renderingDevicePerspectives: television:display;1;1;0;0 diff --git a/worlds/catch_the_bird.wbt b/worlds/catch_the_bird.wbt index ae55d5e..dd3c4d4 100644 --- a/worlds/catch_the_bird.wbt +++ b/worlds/catch_the_bird.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/complete_test.wbt b/worlds/complete_test.wbt index 6e4992a..0c82524 100644 --- a/worlds/complete_test.wbt +++ b/worlds/complete_test.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { @@ -78,6 +78,8 @@ Robot { translation 0 0 0.1 rotation 1 0 0 1.5707963267948966 children [ + Altimeter { + } SliderJoint { device [ Brake { @@ -86,6 +88,7 @@ Robot { LinearMotor { name "linear_motor" maxPosition 100 + multiplier 1.5 } ] endPoint Solid { @@ -109,7 +112,9 @@ Robot { } } } - HingeJoint { + DEF HINGE_JOINT HingeJoint { + jointParameters HingeJointParameters { + } device [ PositionSensor { name "position_sensor" @@ -143,6 +148,7 @@ Robot { name "rotational_motor" acceleration 5 maxVelocity 2 + multiplier 2.5 } ] endPoint DEF TEST2 Solid { @@ -222,7 +228,7 @@ Robot { } InertialUnit { name "inertial_unit" - noise 0.00001 + noise 1e-05 } LED { translation 0.1 0.05 0.1 diff --git a/worlds/e-puck_line.wbt b/worlds/e-puck_line.wbt index a65e11a..13fc50d 100644 --- a/worlds/e-puck_line.wbt +++ b/worlds/e-puck_line.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { @@ -16,7 +16,7 @@ RectangleArena { floorAppearance PBRAppearance { baseColorMap ImageTexture { url [ - "textures/floor.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/robots/gctronic/e-puck/worlds/textures/floor.png" ] } roughness 1 diff --git a/worlds/keyboard_teleop.wbt b/worlds/keyboard_teleop.wbt index d7a9f4a..afd0b69 100644 --- a/worlds/keyboard_teleop.wbt +++ b/worlds/keyboard_teleop.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/panoramic_view_recorder.wbt b/worlds/panoramic_view_recorder.wbt index f6e5f3d..4a48272 100644 --- a/worlds/panoramic_view_recorder.wbt +++ b/worlds/panoramic_view_recorder.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/pioneer3at.wbt b/worlds/pioneer3at.wbt index 53fb69c..f14eab6 100644 --- a/worlds/pioneer3at.wbt +++ b/worlds/pioneer3at.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { info [ "Pioneer 3AT robot (Adept MobileRobots)." @@ -712,20 +712,16 @@ DEF KITCHEN Transform { ] } DEF PIONEER_3AT Pioneer3at { - translation 1.69482 -4.10279 0.11 - rotation 1 0 0 1.5707963267948966 + translation 1.69482 -4.10279 0 + rotation 1 0 0 4.692820414042842e-06 controller "ros" controllerArgs [ "--name=pioneer3at" + "--use-ros-control" + "--auto-publish" + "--robot-description" ] extensionSlot [ - Camera { - translation 0 0.17 -0.22 - width 256 - height 128 - motionBlur 500 - noise 0.02 - } Accelerometer { lookupTable [ -39.24 -39.24 0.005 @@ -739,13 +735,13 @@ DEF PIONEER_3AT Pioneer3at { ] } SickLms291 { - translation 0 0.23 -0.136 + translation 0.136 0 0.35 + rotation -0.5773509358554485 0.5773489358556708 0.5773509358554485 -2.094395307179586 noise 0.1 } GPS { } InertialUnit { - rotation -0.577349935856137 0.577349935856137 0.5773509358560258 2.09439 } ] } diff --git a/worlds/ros_python.wbt b/worlds/ros_python.wbt index e47496b..99f9a22 100644 --- a/worlds/ros_python.wbt +++ b/worlds/ros_python.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { info [ "Simple Thymio II simulation controlled by a ROS node written in Python." diff --git a/worlds/textures/floor.png b/worlds/textures/floor.png deleted file mode 100644 index c80c394..0000000 Binary files a/worlds/textures/floor.png and /dev/null differ diff --git a/worlds/tiago.wbt b/worlds/tiago.wbt new file mode 100644 index 0000000..c99e5bf --- /dev/null +++ b/worlds/tiago.wbt @@ -0,0 +1,546 @@ +#VRML_SIM R2021b utf8 +WorldInfo { + info [ + "TIAGo Steel simulation with MoveIt and differential drive integration (ROS)." + ] + title "TIAGo Steel" + basicTimeStep 16 + contactProperties [ + ContactProperties { + softCFM 1e-05 + } + ] +} +Viewpoint { + orientation 0.4925721069109625 0.36674151919891895 0.7892232748652084 1.5125517327991547 + position 15.600358335111576 -8.652535001134972 11.754842948879098 + exposure 0.5 +} +TexturedBackground { + texture "empty_office" + skybox FALSE + skyColor [ + 0.2 0.2 0.2 + ] +} +DEF FLOOR Solid { + translation 3.3 -4.95 0 + rotation 1 0 0 1.5707963267948966 + children [ + Shape { + appearance Parquetry { + textureTransform TextureTransform { + scale 6 9 + } + } + geometry DEF FLOOR_PLANE Plane { + size 6.6 9.9 + } + } + ] + name "floor" + boundingObject USE FLOOR_PLANE + locked TRUE +} +DEF ROOF Solid { + translation 3.3 -4.95 2.4 + rotation 1 0 0 4.712386326794896 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.8 0.8 0.8 + baseColorMap ImageTexture { + url [ + "textures/roughcast.jpg" + ] + } + roughness 0.5 + metalness 0 + textureTransform TextureTransform { + scale 8 8 + } + } + geometry USE FLOOR_PLANE + } + ] + name "roof" + boundingObject USE FLOOR_PLANE + locked TRUE +} +Wall { + translation 0.65 0 0 + rotation 1 0 0 1.5707963267948966 + name "wall 1" + size 1 2.4 0.3 +} +Window { + translation 1.65 0 0 + rotation 1 0 0 1.5707963267948966 + name "window 1" + size 1 2.4 0.3 + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } +} +Wall { + translation 4.3 0 0 + rotation 1 0 0 1.5707963267948966 + name "wall 2" + size 4.3 2.4 0.3 +} +Wall { + translation 2.3 -9.9 0 + rotation 1 0 0 1.5707963267948966 + name "wall 3" + size 4.3 2.4 0.3 +} +Wall { + translation 5.95 -9.9 0 + rotation 1 0 0 1.5707963267948966 + name "wall 4" + size 1 2.4 0.3 +} +Door { + translation 4.95 -9.9 0 + rotation 9.38185669e-07 0.707106781186236 0.707106781186236 3.141590777218456 + name "door 1" + size 1 2.4 0.3 + canBeOpen FALSE + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + doorHandle DoorLever { + hasStaticParent TRUE + } +} +Wall { + translation 0 -3.8 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 5" + size 7.9 2.4 0.3 +} +Wall { + translation 0 -9.4 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 6" + size 1.3 2.4 0.3 +} +Window { + translation 0 -8.25 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "window 2" + size 1 2.4 0.3 + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } +} +Wall { + translation 6.6 -9.4 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 7" + size 1.3 2.4 0.3 +} +Door { + translation 6.6 -8.25 0 + rotation 0.577348855372322 -0.577350976096979 -0.577350976096979 2.094397223120449 + name "door 2" + size 1 2.4 0.3 + canBeOpen FALSE + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + doorHandle DoorLever { + rotation -0.0012868889344011497 0.9999991719580925 0 0 + hasStaticParent TRUE + } +} +Wall { + translation 6.6 -3.8 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 8" + size 7.9 2.4 0.3 +} +Wall { + translation 1.8 -3.3 0 + rotation 1 0 0 1.5707963267948966 + name "wall 9" + size 3.3 2.4 0.3 +} +CeilingLight { + translation 2.47061 -1.3341 2.4 + rotation 1 0 0 1.5707963267948966 + name "ceiling light 1" + pointLightIntensity 5 +} +CeilingLight { + translation 2.44317 -7.10107 2.4 + rotation 1 0 0 1.5707963267948966 + name "ceiling light 2" + pointLightIntensity 8 +} +FloorLight { + translation 0.745582 -4.00427 0 + rotation 1 0 0 1.5707963267948966 + pointLightIntensity 2 +} +Fridge { + translation 0.5 -0.52 0 + rotation 1 0 0 1.5707963267948966 + mainColor 0.6666666666666666 0 0 +} +Cabinet { + translation 0.15 -1.31 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "cabinet 1" + depth 0.68 + outerThickness 0.02 + rowsHeights [ + 0.22, 0.21, 0.21 + ] + columnsWidths [ + 0.42, 0.42 + ] + layout [ + "Drawer (1, 1, 1, 1, 1.5)" + "Drawer (1, 2, 1, 1, 1.5)" + "Drawer (1, 3, 1, 1, 1.5)" + "Shelf (1, 2, 2, 0)" + "Shelf (1, 3, 2, 0)" + "Shelf (1, 1, 0, 3)" + ] + handle CabinetHandle { + handleLength 0.09 + handleRadius 0.008 + } + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } +} +Cabinet { + translation 0.150001 -1.31 1.12 + rotation -0.577350661639742 -0.577350072964468 -0.577350072964468 4.188790793461465 + name "cabinet 2" + outerThickness 0.02 + rowsHeights [ + 0.22, 0.21, 0.21 + ] + columnsWidths [ + 0.42, 0.42 + ] + layout [ + "RightSidedDoor (1, 1, 1, 3, 1.5)" + "LeftSidedDoor (2, 1, 1, 3, 1.5)" + "Shelf (1, 2, 2, 0)" + "Shelf (1, 3, 2, 0)" + "Shelf (1, 1, 0, 3)" + ] + handle CabinetHandle { + translation -0.2 0 0 + handleLength 0.09 + handleRadius 0.008 + } + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } +} +Cabinet { + translation 0.15 -2.19 0 + rotation -0.577350661639742 -0.577350072964468 -0.577350072964468 4.188790793461465 + name "cabinet 3" + depth 0.68 + outerThickness 0.02 + rowsHeights [ + 0.22, 0.21, 0.21 + ] + columnsWidths [ + 0.42, 0.42 + ] + layout [ + "LeftSidedDoor (2, 1, 1, 3, 1.5)" + "Shelf (1, 2, 2, 0)" + "Shelf (1, 3, 2, 0)" + "Shelf (1, 1, 0, 3)" + ] + handle CabinetHandle { + translation 0.2 0 0 + handleLength 0.09 + handleRadius 0.008 + } + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } +} +Oven { + translation 0.58 -2.85 0.34 + rotation 1 0 0 1.5707963267948966 +} +Worktop { + translation 0.5 -1.31 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 1" + size 0.88 0.06 0.7 +} +Worktop { + translation 0.807 -1.97 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 2" + size 0.44 0.06 0.086 +} +Worktop { + translation 0.272 -1.97 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 3" + size 0.44 0.06 0.244 +} +Worktop { + translation 0.5 -2.63 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 4" + size 0.88 0.06 0.7 +} +Sink { + translation 0.48 -1.97 0.715 + rotation -0.577350661639742 0.577350072964468 0.577350072964468 4.188790793461465 +} +HotPlate { + translation 0.5 -2.85 0.71 + rotation -0.577350661639742 0.577350072964468 0.577350072964468 4.188790793461465 +} +Can { + translation 0.632793 -0.566328 0.841066 + rotation 1 0 0 1.5707963267948966 + name "can 1" +} +Table { + translation 4.94438 -1.07424 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "table(1)" +} +Chair { + translation 5.41278 -1.46063 -0.00224453 + rotation 3.2757990828e-05 0.707106780807154 0.707106780807154 3.141527137608161 + name "chair 1" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +Chair { + translation 4.44435 -0.642495 -0.00224453 + rotation 1 0 0 1.5707963267948966 + name "chair 2" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +Chair { + translation 4.48748 -1.39428 -0.00224453 + rotation 1 0 0 1.5707963267948966 + name "chair 3" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +Chair { + translation 5.38086 -0.706899 -0.00224453 + rotation 3.2757990828e-05 0.707106780807154 0.707106780807154 3.141527137608161 + name "chair 4" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +FruitBowl { + translation 4.88063 -0.715471 0.739784 + rotation 0.810579112476972 -0.414102343881343 -0.414102343881343 1.779275824851248 + color 0.6666666666666666 0 0 +} +Orange { + translation 4.86999 -0.775472 0.799671 + rotation 1 0 0 1.5707963267948966 +} +Orange { + translation 4.80474 -0.699693 0.799666 + rotation 1 0 0 1.5707963267948966 + name "orange 2" +} +Apple { + translation 4.95672 -0.722921 0.799667 + rotation 1 0 0 1.5707963267948966 +} +Apple { + translation 4.89356 -0.635999 0.79966 + rotation 1 0 0 1.5707963267948966 + name "apple 2" +} +Desk { + translation 0.512574 -5.12497 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 +} +WoodenChair { + translation 0.9 -5.15 0 + rotation 0.577348855372322 -0.577350976096979 -0.577350976096979 2.094397223120449 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +Book { + translation 0.592219 -4.69758 0.719882 + rotation -3.2225617226e-05 5.987464127e-05 0.999999997688268 1.143847227025768 + name "book(1)" +} +Table { + translation 2.55544 -7.16302 0.02 + rotation 1 0 0 1.5707963267948966 + size 1.2 0.53 0.8 +} +Armchair { + translation 3.82671 -5.8922 0 + rotation 0.299531095036754 -0.674641061271134 -0.674641061271134 2.559539551711696 + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 +} +Sofa { + translation 0.804228 -7.05325 0 + rotation 1 0 0 1.5707963267948966 + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 +} +Sofa { + translation 2.49729 -8.95734 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "sofa 2" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 +} +Carpet { + translation 2.55076 -7.14218 -0.015 + rotation 1 0 0 1.5707963267948966 + color 0.13725490196078433 0.13725490196078433 0.13725490196078433 +} +BunchOfSunFlowers { + translation 3.9144 -9.05979 0 + rotation 1 0 0 1.5707963267948966 +} +Book { + translation 0.596131 -4.69173 0.739852 + rotation -0.00015528242257 0.000149611007892 0.999999976751958 0.516204925088763 +} +Can { + translation 2.74188 -7.22627 0.611066 + rotation 0.683001177874697 -0.516483006023323 -0.516483006023323 1.943140607471219 + name "can 2" +} +Can { + translation 2.7233 -7.34994 0.611066 + rotation 1 0 0 1.5707963267948966 + name "can 3" +} +Can { + translation 2.8744 -7.20688 0.611066 + rotation 0.672788985489524 0.5231419410657 0.5231419410657 1.957134326001526 + name "can 4" +} +E-puck { + translation 2.69279 -7.16904 0.549968 + rotation 0.53756105030843 -0.597868502276827 -0.594627085808039 2.159614696165064 + groundSensorsSlot [ + E-puckGroundSensors { + } + ] +} +Cabinet { + translation 6.4326 -3.46807 0 + rotation -0.577350661639742 0.577350072964468 0.577350072964468 4.188790793461465 + name "cabinet 4" + depth 0.4 + outerThickness 0.02 + rowsHeights [ + 0.52, 0.44, 0.44, 0.44, 0.44 + ] + columnsWidths [ + 0.96 + ] + layout [ + "Shelf (1, 2, 1, 0)" + "Shelf (1, 3, 1, 0)" + "Shelf (1, 4, 1, 0)" + "Shelf (1, 5, 1, 0)" + ] + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } +} +Book { + translation 6.15092 -3.02231 1.52924 + rotation 1 0 0 1.322112326794896 + name "book 2" +} +Book { + translation 6.15759 -3.04469 1.529 + rotation 1 0 0 1.305368326794896 + name "book 3" +} +Book { + translation 6.16417 -3.06681 1.52878 + rotation 1 0 0 1.292116326794897 + name "book 4" +} +Book { + translation 6.15821 -3.0912 1.52813 + rotation 1 0 0 1.259078326794896 + name "book 5" +} +Book { + translation 6.16234 -3.11388 1.52779 + rotation 1 0 0 1.244082326794896 + name "book 6" +} +PottedTree { + translation 6.08049 -4.51701 0 + rotation 1 0 0 1.5707963267948966 +} +PortraitPainting { + translation 2.47 -9.73 1.6 + rotation 9.38185669e-07 0.707106781186236 0.707106781186236 3.141590777218456 +} +LandscapePainting { + translation 4.92523 -0.184484 1.62868 + rotation 1 0 0 1.5707963267948966 +} +CreateWall { + translation 3.53117 -3.235 0 + rotation 1 0 0 1.5707963267948966 +} +TiagoSteel { + translation 2.194 -4.717 0.095 + rotation 1 0 0 4.692820414042842e-06 + controller "ros" + controllerArgs [ + "--name=tiago" + "--use-ros-control" + "--auto-publish" + "--robot-description" + ] +}