From: Walter Fetter Lages Date: Wed, 21 Mar 2018 16:47:51 +0000 (-0300) Subject: twil_ident: X-Git-Tag: rosbook2018initial~2 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=fb2870cac101f8df15a0c9695f7fb3278c96711a;p=twil.git twil_ident: enable Eigen in CMakeLists.txt twil_controllers: use namespace in cart_linearizing.launch add script to test openloop add twil_gazebo_ros_control --- diff --git a/twil_controllers/launch/cart_linearizing.launch b/twil_controllers/launch/cart_linearizing.launch index 65803f7..8e08ffe 100644 --- a/twil_controllers/launch/cart_linearizing.launch +++ b/twil_controllers/launch/cart_linearizing.launch @@ -11,6 +11,6 @@ - + diff --git a/twil_controllers/scripts/test_openloop.sh b/twil_controllers/scripts/test_openloop.sh new file mode 100755 index 0000000..e568f67 --- /dev/null +++ b/twil_controllers/scripts/test_openloop.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +rostopic pub -1 /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.5" +rostopic pub -1 /twil/right_wheel_joint_effort_controller/command std_msgs/Float64 "0.5" +sleep 3 +rostopic pub -1 /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" +rostopic pub -1 /twil/right_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" diff --git a/twil_gazebo_ros_control/CMakeLists.txt b/twil_gazebo_ros_control/CMakeLists.txt new file mode 100644 index 0000000..2829f2f --- /dev/null +++ b/twil_gazebo_ros_control/CMakeLists.txt @@ -0,0 +1,165 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_gazebo_ros_control) + +## 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 + gazebo + gazebo_ros_control + twil_description +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES twil_gazebo_ros_control + CATKIN_DEPENDS controller_manager pluginlib gazebo_ros_control twil_description +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${GAZEBO_INCLUDE_DIRS} +) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(twil_gazebo_ros_control + src/twil_robot_hw_sim.cpp +) + +## Declare a cpp executable +# add_executable(twil_gazebo_ros_control_node src/twil_gazebo_ros_control_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(twil_gazebo_ros_control_node twil_gazebo_ros_control_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_gazebo_ros_control.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/twil_gazebo_ros_control/package.xml b/twil_gazebo_ros_control/package.xml new file mode 100644 index 0000000..6f2f997 --- /dev/null +++ b/twil_gazebo_ros_control/package.xml @@ -0,0 +1,66 @@ + + + twil_gazebo_ros_control + 2.0.0 + The twil_gazebo_ros_control package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + + gazebo + gazebo_ros_control + controller_manager + pluginlib + twil_description + + controller_manager + pluginlib + + gazebo_ros_control + twil_description + + + + + + + + + + + \ No newline at end of file diff --git a/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp b/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp new file mode 100644 index 0000000..a21f668 --- /dev/null +++ b/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp @@ -0,0 +1,105 @@ +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace twil_gazebo_ros_control +{ + + class TwilRobotHWSim:public gazebo_ros_control::RobotHWSim + { + + unsigned int n_dof_; + + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + + std::vector joint_name_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_effort_command_; + + std::vector sim_joints_; + + public: + + TwilRobotHWSim(void):n_dof_(2),joint_name_(n_dof_),joint_position_(n_dof_), + joint_velocity_(n_dof_),joint_effort_(n_dof_),joint_effort_command_(n_dof_) + { + + joint_name_[0]="left_wheel_joint"; + joint_name_[1]="right_wheel_joint"; + + for(unsigned int j=0;j < n_dof_;j++) + { + joint_position_[j]=0.0; + joint_velocity_[j]=0.0; + joint_effort_[j]=0.0; + + joint_effort_command_[j] = 0.0; + + js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_name_[j],&joint_position_[j], + &joint_velocity_[j],&joint_effort_[j])); + + joint_effort_command_[j] = 0.0; + ej_interface_.registerHandle(hardware_interface::JointHandle(js_interface_.getHandle(joint_name_[j]), + &joint_effort_command_[j])); + + } + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + } + + + bool initSim(const std::string& robot_namespace, + ros::NodeHandle nh,gazebo::physics::ModelPtr model, + const urdf::Model *const urdf_model, + std::vector transmissions) + { + for(unsigned int j=0;j < n_dof_;j++) + { + ROS_INFO_STREAM("Getting pointer to gazebo joint: " << joint_name_[j]); + gazebo::physics::JointPtr joint=model->GetJoint(joint_name_[j]); + if(joint) sim_joints_.push_back(joint); + else + { + ROS_ERROR_STREAM("This robot has a joint named \"" << joint_name_[j] + <<"\" which is not in the gazebo model."); + return false; + } + } + return true; + } + + void readSim(ros::Time time,ros::Duration period) + { + for(unsigned int j=0; j < n_dof_;j++) + { +// joint_position_[j]+=angles::shortest_angular_distance +// (joint_position_[j],sim_joints_[j]->GetAngle(0).GetAsRadian()); + joint_position_[j]=sim_joints_[j]->GetAngle(0).Radian(); + joint_velocity_[j]=sim_joints_[j]->GetVelocity(0); +// joint_effort_[j]=sim_joints_[j]->GetForce(0); + joint_effort_[j]=joint_effort_command_[j]; + } + } + + void writeSim(ros::Time time,ros::Duration period) + { + for(unsigned int j=0;j < n_dof_;j++) sim_joints_[j]->SetForce(0,joint_effort_command_[j]); + } + + }; +} + +PLUGINLIB_EXPORT_CLASS(twil_gazebo_ros_control::TwilRobotHWSim,gazebo_ros_control::RobotHWSim) diff --git a/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml b/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml new file mode 100644 index 0000000..7d03185 --- /dev/null +++ b/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml @@ -0,0 +1,12 @@ + + + + + A ROS/Gazebo interface Twil, exporting a joint_state_interface and a + joint_effort_interface. + + + diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt index ad82545..0521d8a 100644 --- a/twil_ident/CMakeLists.txt +++ b/twil_ident/CMakeLists.txt @@ -4,7 +4,7 @@ project(twil_ident) ## 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) +find_package(catkin REQUIRED COMPONENTS roscpp) find_package(cmake_modules REQUIRED) @@ -103,17 +103,17 @@ include_directories( # ) ## Declare a cpp executable -# add_executable(twil_ident_node src/twil_ident_node.cpp) +add_executable(ident src/ident.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes # add_dependencies(twil_ident_node twil_ident_generate_messages_cpp) ## Specify libraries to link a library or executable target against -# target_link_libraries(twil_ident_node -# ${catkin_LIBRARIES} -# ${eigen_LIBRARIES} -# ) +target_link_libraries(ident + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} +) ############# ## Install ## @@ -130,7 +130,7 @@ include_directories( # ) ## Mark executables and/or libraries for installation -install(TARGETS twil_ident twil_ident_node +install(TARGETS ident # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}