<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/twil" args="joint_state_controller cart_linearizing_controller"/>
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="/twil" />
</launch>
--- /dev/null
+#!/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"
--- /dev/null
+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)
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>twil_gazebo_ros_control</name>
+ <version>2.0.0</version>
+ <description>The twil_gazebo_ros_control package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_gazebo_ros_control</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>gazebo</build_depend>
+ <build_depend>gazebo_ros_control</build_depend>
+ <build_depend>controller_manager</build_depend>
+ <build_depend>pluginlib</build_depend>
+ <build_depend>twil_description</build_depend>
+
+ <run_depend>controller_manager</run_depend>
+ <run_depend>pluginlib</run_depend>
+ <!--run_depend>gazebo</run_depend-->
+ <run_depend>gazebo_ros_control</run_depend>
+ <run_depend>twil_description</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
\ No newline at end of file
--- /dev/null
+#include <pluginlib/class_list_macros.h>
+
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+
+#include <gazebo_ros_control/robot_hw_sim.h>
+
+#include <angles/angles.h>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+
+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<std::string> joint_name_;
+ std::vector<double> joint_position_;
+ std::vector<double> joint_velocity_;
+ std::vector<double> joint_effort_;
+ std::vector<double> joint_effort_command_;
+
+ std::vector<gazebo::physics::JointPtr> 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<transmission_interface::TransmissionInfo> 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)
--- /dev/null
+<library path="lib/libtwil_gazebo_ros_control">
+
+ <class
+ name="twil_gazebo_ros_control/TwilRobotHWSim"
+ type="twil_gazebo_ros_control::TwilRobotHWSim"
+ base_class_type="gazebo_ros_control::RobotHWSim">
+ <description>
+ A ROS/Gazebo interface Twil, exporting a joint_state_interface and a
+ joint_effort_interface.
+ </description>
+ </class>
+</library>
## 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)
# )
## 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 ##
# )
## 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}
projects / twil.git / commitdiff