<!--
<run_depend>twil_gazebo_ros_control</run_depend>
-->
- <run_depend>twil_controllers</run_depend>
<run_depend>twil_ident</run_depend>
-<!--
- <run_depend>twil_trajectories</run_depend>
--->
<!-- The export tag contains other, unspecified, tags -->
<export>
+++ /dev/null
-cmake_minimum_required(VERSION 2.8.3)
-project(twil_controllers)
-
-## 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
- controller_interface
- effort_controllers
- kdl_parser
- geometry_msgs
- nav_msgs
- tf
- message_generation
-)
-find_package(cmake_modules REQUIRED)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-find_package(Eigen REQUIRED)
-
-
-## 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
- NonSmoothBackstepControllerStatus.msg
- PosePolar.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
- geometry_msgs
- nav_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 ${PROJECT_NAME}
- CATKIN_DEPENDS message_runtime
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(include
- ${catkin_INCLUDE_DIRS}
- ${Eigen_INCLUDE_DIRS}
-)
-
-## Declare a cpp library
-add_library(twil_controllers
- src/cart_linearizing_controller.cpp
- src/cart_linearizing_controller_rne.cpp
- src/nonsmooth_backstep_controller.cpp
-)
-
-## Declare a cpp executable
-# add_executable(twil_controllers_node src/twil_controllers_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_controllers twil_controllers_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_controllers.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
-twil:
-
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 100
-
- left_wheel_joint_effort_controller:
- type: effort_controllers/JointEffortController
- joint: left_wheel_joint
-
- right_wheel_joint_effort_controller:
- type: effort_controllers/JointEffortController
- joint: right_wheel_joint
+++ /dev/null
-twil:
-
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 100
-
- cart_linearizing_controller:
- type: twil_controllers/CartLinearizingController
- joints:
- - left_wheel_joint
- - right_wheel_joint
+++ /dev/null
-# Watch-out: The indentation here is relevant to the semantic!
-
-twil:
-
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 100
-
- nonsmooth_backstep_controller:
- type: twil_controllers/NonSmoothBackstepController
- joints:
- - left_wheel_joint
- - right_wheel_joint
- lambda: [10.0, 0.1, 0.1, 50.0, 100.0]
- gamma: [10.0, 1.0, 10.0, 10.0]
+++ /dev/null
-twil:
-
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 100
-
- left_wheel_joint_velocity_controller:
- type: effort_controllers/JointVelocityController
- joint: left_wheel_joint
- pid: {p: 0.0, i: 0.0, d: 0.0}
-
- right_wheel_joint_velocity_controller:
- type: effort_controllers/JointVelocityController
- joint: right_wheel_joint
- pid: {p: 0.0, i: 0.0, d: 0.0}
-
+++ /dev/null
-#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
-#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <ros/node_handle.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <std_msgs/Float64MultiArray.h>
-#include <sensor_msgs/JointState.h>
-
-#include <Eigen/Dense>
-
-namespace twil_controllers
-{
- class CartLinearizingController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
- {
- public:
- CartLinearizingController(void);
- ~CartLinearizingController(void);
-
- bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
- void starting(const ros::Time& time);
- void update(const ros::Time& time,const ros::Duration& duration);
-
- private:
- ros::NodeHandle node_;
- hardware_interface::EffortJointInterface *robot_;
- std::vector<hardware_interface::JointHandle> joints_;
-
- void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
- ros::Subscriber sub_command_;
-
- Eigen::Vector2d nu;
- Eigen::Vector2d torque;
- Eigen::Vector2d v;
- Eigen::Vector2d u;
-
- Eigen::Matrix2d Ginv;
- Eigen::Matrix2d F;
-
- std::vector<double> wheelRadius;
- double wheelBase;
- };
-}
-#endif
+++ /dev/null
-#ifndef TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H
-#define TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <Eigen/Dense>
-#include <boost/scoped_ptr.hpp>
-
-#include <ros/node_handle.h>
-
-#include <std_msgs/Float64MultiArray.h>
-#include <geometry_msgs/Pose2D.h>
-#include <nav_msgs/Odometry.h>
-#include <tf/tfMessage.h>
-
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <realtime_tools/realtime_publisher.h>
-
-#include <twil_controllers/NonSmoothBackstepControllerStatus.h>
-
-namespace twil_controllers
-{
- class NonSmoothBackstepController: public controller_interface::
- Controller<hardware_interface::EffortJointInterface>
- {
- public:
- NonSmoothBackstepController(void);
- ~NonSmoothBackstepController(void);
-
- bool init(hardware_interface::EffortJointInterface *robot,
- ros::NodeHandle &n);
- void starting(const ros::Time& time);
- void update(const ros::Time& time,const ros::Duration& duration);
-
- private:
- ros::NodeHandle node_;
- hardware_interface::EffortJointInterface *robot_;
- std::vector<hardware_interface::JointHandle> joints_;
-
- boost::scoped_ptr<realtime_tools::RealtimePublisher
- <twil_controllers::NonSmoothBackstepControllerStatus>
- > status_publisher_ ;
-
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <nav_msgs::Odometry> > odom_publisher_;
- boost::shared_ptr<realtime_tools::RealtimePublisher
- <tf::tfMessage> > tf_odom_publisher_;
-
- ros::Subscriber sub_command_;
- ros::Subscriber sub_parameters_;
-
- Eigen::Matrix2d Ginv;
- Eigen::Matrix2d F;
-
- std::vector<double> wheelRadius;
- double wheelBase;
-
- Eigen::Vector3d x;
- Eigen::Vector3d xRef;
-
- Eigen::Vector2d eta;
-
- ros::Time lastSamplingTime;
-
- Eigen::Vector2d phi;
-
- std::vector<double> lambda;
- std::vector<double> gamma;
-
- void commandCB(const geometry_msgs::Pose2D::ConstPtr &command);
- void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command);
- };
-}
-#endif
+++ /dev/null
-<launch>
- <rosparam file="$(find twil_controllers)/config/linearizing_control.yaml" command="load"/>
-
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="twil" args="joint_state_controller cart_linearizing_controller"/>
-</launch>
+++ /dev/null
-<launch>
- <arg name="paused" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="use_sim_time" default="true"/>
- <arg name="wam" default="false"/>
-
- <include file="$(find twil_description)/launch/gazebo.launch" >
- <arg name="paused" value="$(arg paused)"/>
- <arg name="headless" value="$(arg headless)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="wam" value="$(arg wam)"/>
- </include>
-
- <include file="$(find twil_controllers)/launch/joint_effort.launch" />
-
-</launch>
+++ /dev/null
-<launch>
- <arg name="paused" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="use_sim_time" default="true"/>
- <arg name="wam" default="false"/>
-
- <remap from="/twil/command" to="/twil/nonsmooth_backstep_controller/command" />
- <remap from="/twil/joint_states" to="/joint_states" />
-
- <include file="$(find twil_description)/launch/gazebo.launch" >
- <arg name="paused" value="$(arg paused)"/>
- <arg name="headless" value="$(arg headless)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="wam" value="$(arg wam)"/>
- </include>
-
- <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
-
- <node name="eight_trajectory" pkg="twil_trajectories" type="eight_trajectory" respawn="false" output="screen" ns="twil" args="_x:=0.0 _y:=-0.5 _radius:=1.0 _ang_vel:=0.1"/>
-
- <node name="pose2d_stamp" pkg="twil_trajectories" type="pose2d_stamp" respawn="false" output="screen" ns="twil" args="_frame_id:=map" />
-
- <node pkg="tf2_ros" type="static_transform_publisher" name="odom_publisher" args="0 0 0 0 0 0 1 map odom" />
-
- <include file="$(find twil_description)/launch/display.launch" />
-
-</launch>
+++ /dev/null
-<launch>
- <rosparam file="$(find twil_controllers)/config/effort_control.yaml" command="load"/>
-
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="twil" args="left_wheel_joint_effort_controller right_wheel_joint_effort_controller joint_state_controller"/>
-</launch>
+++ /dev/null
-<launch>
- <rosparam file="$(find twil_controllers)/config/velocity_control.yaml" command="load"/>
-
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="twil" args="joint_state_controller left_wheel_joint_velocity_controller right_wheel_joint_velocity_controller"/>
-</launch>
+++ /dev/null
-<launch>
- <rosparam file="$(find twil_controllers)/config/nonsmooth_backstep_control.yaml" command="load"/>
-
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="twil" args="joint_state_controller nonsmooth_backstep_controller"/>
-</launch>
+++ /dev/null
-Header header
-geometry_msgs/Pose2D set_point
-geometry_msgs/Pose2D process_value
-geometry_msgs/Pose2D process_value_dot
-geometry_msgs/Pose2D error
-float64 time_step
-float64[2] command
-float64[5] lambda
-float64[4] gamma
-PosePolar polar_error
-float64[2] backstep_set_point
-float64[2] backstep_set_point_dot
-float64[2] backstep_process_value
-float64[2] backstep_error
-float64[2] backstep_command
-float64[2] linear_dynamics_command
+++ /dev/null
-float64 range
-float64 angle
-float64 orientation
+++ /dev/null
-<?xml version="1.0"?>
-<package>
- <name>twil_controllers</name>
- <version>2.0.0</version>
- <description>The twil_controllers 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_controllers</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>controller_interface</build_depend>
- <build_depend>effort_controllers</build_depend>
- <build_depend>kdl_parser</build_depend>
- <build_depend>geometry_msgs</build_depend>
- <build_depend>nav_msgs</build_depend>
- <build_depend>tf</build_depend>
- <build_depend>control_toolbox</build_depend>
- <build_depend>realtime_tools</build_depend>
- <build_depend>message_generation</build_depend>
-
- <run_depend>controller_interface</run_depend>
- <run_depend>controller_manager</run_depend>
- <run_depend>effort_controllers</run_depend>
- <run_depend>joint_state_controller</run_depend>
- <run_depend>kdl_parser</run_depend>
- <run_depend>geometry_msgs</run_depend>
- <run_depend>nav_msgs</run_depend>
- <run_depend>tf</run_depend>
- <run_depend>control_toolbox</run_depend>
- <run_depend>realtime_tools</run_depend>
- <run_depend>message_runtime</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 -->
- <controller_interface plugin="${prefix}/twil_controllers_plugins.xml"/>
- </export>
-</package>
+++ /dev/null
-#!/bin/bash
-
-rostopic pub -1 /twil/nonsmooth_backstep_controller/command geometry_msgs/Pose2D "{x: $1, y: $2, theta: $3}"
+++ /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
-#include <twil_controllers/cart_linearizing_controller.h>
-#include <pluginlib/class_list_macros.h>
-
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-
-namespace twil_controllers
-{
- CartLinearizingController::CartLinearizingController(void):
- wheelRadius(2)
- {
- }
-
- CartLinearizingController::~CartLinearizingController(void)
- {
- sub_command_.shutdown();
- }
-
- bool CartLinearizingController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
- {
- node_=n;
- robot_=robot;
-
- XmlRpc::XmlRpcValue joint_names;
- if(!node_.getParam("joints",joint_names))
- {
- ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
- {
- ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- for(int i=0; i < joint_names.size();i++)
- {
- XmlRpc::XmlRpcValue &name_value=joint_names[i];
- if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
- {
- ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- hardware_interface::JointHandle j=robot->getHandle((std::string)name_value);
- joints_.push_back(j);
- }
- sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,this);
-
- std::string robot_desc_string;
- if(!node_.getParam("/robot_description",robot_desc_string))
- {
- ROS_ERROR("Could not find '/robot_description'.");
- return false;
- }
-
- KDL::Tree tree;
- if (!kdl_parser::treeFromString(robot_desc_string,tree))
- {
- ROS_ERROR("Failed to construct KDL tree.");
- return false;
- }
-
- // get wheelBase from URDF (actually from KDL tree)
- KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support");
- KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("right_wheel_support");
- KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- wheelRadius.resize(joints_.size());
- wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3);
-
- // get wheelRadius from URDF (actually from KDL tree)
- segmentMapIter=tree.getSegment("chassis");
- KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("left_wheel");
- KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z();
-
- segmentMapIter=tree.getSegment("right_wheel");
- KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z();
-
- const double K5 = 0.0018533548425194695;
- const double K6 = 0.09946140462774823;
- const double K7 = 21.65458426501294;
- const double K8 = -15.40102896939387;
- Ginv << 0.5*K7, 0.5*K8,
- 0.5*K7, -0.5*K8;
- F << 0.0, K5,
- K6, 0.0;
-
- return true;
- }
-
- void CartLinearizingController::starting(const ros::Time& time)
- {
- v.setZero();
- }
-
- void CartLinearizingController::update(const ros::Time& time,
- const ros::Duration& duration)
- {
-
- for(unsigned int i=0;i < joints_.size();i++)
- {
- nu[i]=joints_[i].getVelocity();
- }
-
- u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0;
- u[1]=(nu[0]*wheelRadius[0]-nu[1]*wheelRadius[1])/wheelBase;
-
- // Compute linearization
- Eigen::Vector2d uf;
- uf << u[0]*u[1], u[1]*u[1];
-
- torque=Ginv*(v+F*uf);
-
- // Apply torques
- for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i));
- }
-
- void CartLinearizingController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command)
- {
- for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i];
- }
-}
-PLUGINLIB_EXPORT_CLASS(twil_controllers::CartLinearizingController,controller_interface::ControllerBase)
+++ /dev/null
-#include <boost/assign.hpp>
-
-#include <pluginlib/class_list_macros.h>
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-
-#include <twil_controllers/nonsmooth_backstep_controller.h>
-
-#define sqr(x) (x*x)
-#define cub(x) (x*x*x)
-
-namespace twil_controllers
-{
- NonSmoothBackstepController::NonSmoothBackstepController(void):
- wheelRadius(2),lambda(5),gamma(4)
- {
- }
-
- NonSmoothBackstepController::~NonSmoothBackstepController(void)
- {
- sub_command_.shutdown();
- sub_parameters_.shutdown();
- }
-
- bool NonSmoothBackstepController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n)
- {
- node_=n;
- robot_=robot;
-
- XmlRpc::XmlRpcValue joint_names;
- if(!node_.getParam("joints",joint_names))
- {
- ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
- {
- ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- for(int i=0; i < joint_names.size();i++)
- {
- XmlRpc::XmlRpcValue &name_value=joint_names[i];
- if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
- {
- ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- hardware_interface::JointHandle j=robot->getHandle((std::string)name_value);
- joints_.push_back(j);
- }
-
- status_publisher_.reset(new realtime_tools::RealtimePublisher<twil_controllers::NonSmoothBackstepControllerStatus>(node_,"status",1));
- status_publisher_->msg_.header.frame_id="odom";
-
- odom_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(node_,"odom",100));
- odom_publisher_->msg_.header.frame_id="odom";
- odom_publisher_->msg_.child_frame_id="twil";
- odom_publisher_->msg_.pose.pose.position.z=0;
- odom_publisher_->msg_.pose.pose.orientation.x=0;
- odom_publisher_->msg_.pose.pose.orientation.y=0;
-
- // Fake covariance
- double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9};
- odom_publisher_->msg_.pose.covariance=boost::assign::list_of
- (pose_cov[0]) (0) (0) (0) (0) (0)
- (0) (pose_cov[1]) (0) (0) (0) (0)
- (0) (0) (pose_cov[2]) (0) (0) (0)
- (0) (0) (0) (pose_cov[3]) (0) (0)
- (0) (0) (0) (0) (pose_cov[4]) (0)
- (0) (0) (0) (0) (0) (pose_cov[5]);
-
- odom_publisher_->msg_.twist.twist.linear.z=0;
- odom_publisher_->msg_.twist.twist.angular.x=0;
- odom_publisher_->msg_.twist.twist.angular.y=0;
-
- //Fake covariance
- double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9};
- odom_publisher_->msg_.twist.covariance=boost::assign::list_of
- (twist_cov[0]) (0) (0) (0) (0) (0)
- (0) (twist_cov[1]) (0) (0) (0) (0)
- (0) (0) (twist_cov[2]) (0) (0) (0)
- (0) (0) (0) (twist_cov[3]) (0) (0)
- (0) (0) (0) (0) (twist_cov[4]) (0)
- (0) (0) (0) (0) (0) (twist_cov[5]);
-
- tf_odom_publisher_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(node_,"/tf",100));
- tf_odom_publisher_->msg_.transforms.resize(1);
- tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0;
- tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0;
- tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0;
- tf_odom_publisher_->msg_.transforms[0].child_frame_id="twil_origin";
- tf_odom_publisher_->msg_.transforms[0].header.frame_id="odom";
-
- sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this);
-
- sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,this);
-
- std::string robot_desc_string;
- if(!node_.getParam("/robot_description",robot_desc_string))
- {
- ROS_ERROR("Could not find '/robot_description'.");
- return false;
- }
-
- KDL::Tree tree;
- if (!kdl_parser::treeFromString(robot_desc_string,tree))
- {
- ROS_ERROR("Failed to construct KDL tree.");
- return false;
- }
-
- // get wheelBase from URDF (actually from KDL tree)
- KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support");
- KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("right_wheel_support");
- KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- wheelRadius.resize(joints_.size());
- wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3);
-
- // get wheelRadius from URDF (actually from KDL tree)
- segmentMapIter=tree.getSegment("chassis");
- KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("left_wheel");
- KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z();
-
- segmentMapIter=tree.getSegment("right_wheel");
- KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z();
-
- gamma[0]=10.0;
- gamma[1]=1.0;
- gamma[2]=10.0;
- gamma[3]=10.0;
- node_.param("gamma",gamma,gamma);
-
- lambda[0]=10.0;
- lambda[1]=0.1;
- lambda[2]=0.1;
- lambda[3]=50.0;
- lambda[4]=100.0;
- node_.param("lambda",lambda,lambda);
-
- const double K5=0.08444758509282763;
- const double K6=3.770688129256381;
- const double K7=2.6468901285322475;
- const double K8=-16.084061415321404;
-
- Ginv << 0.5/K7, 0.5/K8,
- 0.5/K7, -0.5/K8;
- F << 0.0, K5,
- K6, 0.0;
-
- return true;
- }
-
- void NonSmoothBackstepController::starting(const ros::Time& time)
- {
- x.setZero();
- xRef.setZero();
- eta.setZero();
- lastSamplingTime=time;
- for(unsigned int i=0;i < joints_.size();i++)
- {
- phi[i]=joints_[i].getPosition();
- }
- }
-
- void NonSmoothBackstepController::update(const ros::Time& time,
- const ros::Duration& duration)
- {
- double dt=time.toSec()-lastSamplingTime.toSec();
-
- if(fabs(dt-0.01) > 0.001/2) return;
- lastSamplingTime=time;
-
- // Incremental encoders sense angular displacement and
- // not velocity
- // phi[0] is the left wheel angular displacement
- // phi[1] is the right wheel angular displacement
- Eigen::Vector2d deltaPhi=-phi;
- for(unsigned int i=0;i < joints_.size();i++)
- {
- phi[i]=joints_[i].getPosition();
- }
- deltaPhi+=phi;
-
- // Estimate pose by odometry
- Eigen::Vector2d deltaU;
- deltaU << (deltaPhi[0]*wheelRadius[0]+deltaPhi[1]*wheelRadius[1])/2.0,
- (deltaPhi[1]*wheelRadius[1]-deltaPhi[0]*wheelRadius[0])/wheelBase;
-
- double deltaS=(fabs(deltaU[1]) > 1e-10)?
- deltaU[0]*sin(deltaU[1])/deltaU[1]:deltaU[0];
-
- Eigen::Vector3d deltaX;
- deltaX << deltaS*cos(x[2]+deltaU[1]/2.0),
- deltaS*sin(x[2]+deltaU[1]/2.0),
- deltaU[1];
-
- x+=deltaX;
-
- Eigen::Vector2d u=deltaU/dt;
-
- // Change of coordinates
- Eigen::Matrix3d R;
- R << cos(xRef[2]), sin(xRef[2]), 0.0,
- -sin(xRef[2]), cos(xRef[2]), 0.0,
- 0.0, 0.0, 1.0;
- Eigen::Vector3d xBar=R*(x-xRef);
-
- // Discontinuous transformation
- double e=sqrt(sqr(xBar[0])+sqr(xBar[1]));
- double psi=atan2(xBar[1],xBar[0]);
- double alpha=xBar[2]-psi;
-
- // Backstepping
- eta[0]=-gamma[0]*e*cos(alpha);
-
- if(fabs(alpha ) > 1e-10)
- eta[1]=-gamma[1]*alpha
- -gamma[0]*sin(alpha)*cos(alpha)+gamma[0]*lambda[2]*psi*sin(alpha)/
- lambda[1]/alpha*cos(alpha);
- else eta[1]=gamma[0]*lambda[2]*psi/lambda[1];
-
- Eigen::Vector2d eb=u-eta;
-
- Eigen::Vector2d vBar;
- if(fabs(e) > 1e-10) vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha)
- +lambda[1]/lambda[3]*alpha*sin(alpha)/e
- -lambda[2]/lambda[3]*psi*sin(alpha)/e;
- else vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha);
- vBar[1]=-gamma[3]*eb[1]-lambda[1]/lambda[4]*alpha;
-
- Eigen::Vector2d deta;
-
- deta[0]=e*cub(cos(alpha))*sqr(gamma[0])-e*gamma[0]*sin(alpha)*(alpha*gamma[1]
- -(gamma[0]*lambda[2]*cos(alpha)*sin(alpha)*psi)/(alpha*lambda[1]));
-
- deta[1]=gamma[1]*(alpha*gamma[1]
- -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))
- +gamma[0]*sqr(cos(alpha))*(alpha*gamma[1]
- -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))
- -gamma[0]*sqr(sin(alpha))*(alpha*gamma[1]
- -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))
- -(lambda[2]*sqr(cos(alpha))*sqr(sin(alpha))*sqr(gamma[0]))/(alpha*lambda[1])
- -(gamma[0]*lambda[2]*psi*sqr(cos(alpha))*(alpha*gamma[1]
- -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])))
- /(alpha*lambda[1])
- +(gamma[0]*lambda[2]*psi*sqr(sin(alpha))*(alpha*gamma[1]
- -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])))
- /(alpha*lambda[1])
- +(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha)*(alpha*gamma[1]
- -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])))
- /(sqr(alpha)*lambda[1]);
-
- Eigen::Vector2d v=vBar+deta;
-
- // Linearization
- Eigen::Vector2d uf;
- uf << u[0]*u[1], sqr(u[1]);
-
- Eigen::Vector2d torque=Ginv*(v-F*uf);
-
- // Apply torques
- for(unsigned int i=0;i < joints_.size();i++)
- {
- joints_[i].setCommand(torque[i]);
- }
-
- if(status_publisher_ && status_publisher_->trylock())
- {
- status_publisher_->msg_.header.stamp=time;
-
- status_publisher_->msg_.set_point.x=xRef[0];
- status_publisher_->msg_.set_point.y=xRef[1];
- status_publisher_->msg_.set_point.theta=xRef[2];
-
- status_publisher_->msg_.process_value.x=x[0];
- status_publisher_->msg_.process_value.y=x[1];
- status_publisher_->msg_.process_value.theta=x[2];
-
- status_publisher_->msg_.process_value_dot.x=deltaX[0]/dt;
- status_publisher_->msg_.process_value_dot.y=deltaX[1]/dt;
- status_publisher_->msg_.process_value_dot.theta=deltaX[2]/dt;
-
- status_publisher_->msg_.error.x=xRef[0]-x[0];
- status_publisher_->msg_.error.y=xRef[1]-x[1];
- status_publisher_->msg_.error.theta=xRef[2]-x[2];
-
- status_publisher_->msg_.time_step=dt;
-
- for(int i=0;i < torque.size();i++)
- status_publisher_->msg_.command[i]=torque[i];
-
- for(int i=0;i < lambda.size();i++)
- status_publisher_->msg_.lambda[i]=lambda[i];
-
- for(int i=0;i < gamma.size();i++)
- status_publisher_->msg_.gamma[i]=gamma[i];
-
- status_publisher_->msg_.polar_error.range=e;
- status_publisher_->msg_.polar_error.angle=psi;
- status_publisher_->msg_.polar_error.orientation=alpha;
-
- for(int i=0;i < eta.size();i++)
- status_publisher_->msg_.backstep_set_point[i]=eta[i];
-
- for(int i=0;i < deta.size();i++)
- status_publisher_->msg_.backstep_set_point_dot[i]=deta[i];
-
- for(int i=0;i < u.size();i++)
- status_publisher_->msg_.backstep_process_value[i]=u[i];
-
- for(int i=0;i < eb.size();i++)
- status_publisher_->msg_.backstep_error[i]=eb[i];
-
- for(int i=0;i < vBar.size();i++)
- status_publisher_->msg_.backstep_command[i]=vBar[i];
-
- for(int i=0;i < v.size();i++)
- status_publisher_->msg_.linear_dynamics_command[i]=v[i];
-
- status_publisher_->unlockAndPublish();
- }
-
- if(odom_publisher_ && odom_publisher_->trylock())
- {
- odom_publisher_->msg_.header.stamp=time;
-
- odom_publisher_->msg_.pose.pose.position.x=x[0];
- odom_publisher_->msg_.pose.pose.position.y=x[1];
- odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2);
- odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2);
-
- odom_publisher_->msg_.twist.twist.linear.x=deltaX[0]/dt;
- odom_publisher_->msg_.twist.twist.linear.y=deltaX[1]/dt;
- odom_publisher_->msg_.twist.twist.angular.z=deltaX[2]/dt;
-
- odom_publisher_->unlockAndPublish();
- }
-
- if(tf_odom_publisher_ && tf_odom_publisher_->trylock())
- {
- geometry_msgs::TransformStamped &odom_frame=
- tf_odom_publisher_->msg_.transforms[0];
- odom_frame.header.stamp=time;
- odom_frame.transform.translation.x=x[0];
- odom_frame.transform.translation.y=x[1];
- odom_frame.transform.rotation.z=sin(x[2]/2);
- odom_frame.transform.rotation.w=cos(x[2]/2);
-
- tf_odom_publisher_->unlockAndPublish();
- }
- }
-
- void NonSmoothBackstepController::commandCB(const geometry_msgs::Pose2D::ConstPtr &command)
- {
- xRef[0]=command->x;
- xRef[1]=command->y;
- xRef[2]=command->theta;
- }
-
- void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr ¶m)
- {
- // data[0]=K5, data[1]=K6, data[2]=K7, data[3]=K8
- // data[4]=Pk5, data[5]=Pk6, data[6]=Pk7, data[7]=Pk8
- // F= [0 K5
- // K6 0]
- // Ginv=[0.5/K7 0.5/K8
- // 0.5/K7 -0.5/K8]
-
- if(param->data[4] < 1e-3) F(0,1)=param->data[0];
- if(param->data[5] < 1e-3) F(1,0)=param->data[1];
- if(param->data[6] < 1e-3)
- {
- Ginv(0,0)=0.5/param->data[2];
- Ginv(1,0)=0.5/param->data[2];
- }
- if(param->data[7] < 1e-3)
- {
- Ginv(0,1)=0.5/param->data[3];
- Ginv(1,1)=-0.5/param->data[3];
- }
- }
-
-}
-
-PLUGINLIB_EXPORT_CLASS(twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase)
+++ /dev/null
-<library path="lib/libtwil_controllers">
-
- <class name="twil_controllers/CartLinearizingController" type="twil_controllers::CartLinearizingController" base_class_type="controller_interface::ControllerBase">
- <description>
- The CartLinearizingController linearizes the Twil dynamic model. The
- linearized inputs are linear and angular accelerations. It expects a
- EffortJointInterface type of hardware interface.
- </description>
- </class>
-
- <class name="twil_controllers/CartLinearizingController_RNE" type="twil_controllers::CartLinearizingController" base_class_type="controller_interface::ControllerBase">
- <description>
- The CartLinearizingController linearizes the Twil dynamic model. The
- linearized inputs are linear and angular accelerations. It expects a
- EffortJointInterface type of hardware interface. Recursive
- Newton-Euler from KDL is used to compute torque. Not working
- properly.
- </description>
- </class>
-
- <class name="twil_controllers/NonSmoothBackstepController" type="twil_controllers::NonSmoothBackstepController" base_class_type="controller_interface::ControllerBase">
- <description>
- The NonSmoothBackstepController implements a non-smooth controller
- based on a discontinuous transformation and backstepping. A feedback
- linearization of the Twil dynamic model is also performed. The
- inputs to the controller are the reference Cartesian pose. It expects a
- EffortJointInterface type of hardware interface.
- </description>
- </class>
-
-</library>
<arg name="wam" value="$(arg wam)" />
</include>
+ <node pkg="tf2_ros" type="static_transform_publisher" name="mob_origin_publisher" args="0 0 0 0 0 0 1 map twil_origin" />
+
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" args="_use_gui:=$(arg use_gui)" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find twil_description)/rviz/urdf.rviz" required="true"/>
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find twil_description)/rviz/display.rviz" required="true"/>
</launch>
Show Axes: false
Show Trail: false
Value: true
- castor_base:
+ caster_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- castor_support:
+ caster_support:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- castor_wheel:
+ caster_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Show Axes: false
Show Trail: false
Value: true
- front_castor_base:
+ front_caster_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- front_castor_support:
+ front_caster_support:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- front_castor_wheel:
+ front_caster_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Length: 1
Name: Odometry
Position Tolerance: 0.3
- Topic: /twil/nonsmooth_backstep_controller/odom
+ Topic: /nonsmooth_backstep_controller/odom
Value: true
- Alpha: 1
Axes Length: 1
Shaft Length: 1
Shaft Radius: 0.05
Shape: Arrow
- Topic: /twil/command_stamped
+ Topic: /command_stamped
Value: true
Enabled: true
Global Options:
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:macro name="castor_base" params="name parent *origin">
+ <xacro:macro name="caster_base" params="name parent *origin">
<link name="${name}">
<inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package://twil_description/meshes/castor_base.stl" />
+ <mesh filename="package://twil_description/meshes/caster_base.stl" />
</geometry>
<material name="">
<color rgba="0.64706 0.61961 0.58431 1" />
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package://twil_description/meshes/castor_base.stl" />
+ <mesh filename="package://twil_description/meshes/caster_base.stl" />
</geometry>
</collision>
</link>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:macro name="castor_support" params="name parent *origin">
+ <xacro:macro name="caster_support" params="name parent *origin">
<link name="${name}">
<inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package://twil_description/meshes/castor_support.stl" />
+ <mesh filename="package://twil_description/meshes/caster_support.stl" />
</geometry>
<material name="">
<color rgba="0.64706 0.61961 0.58431 1" />
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package://twil_description/meshes/castor_support.stl" />
+ <mesh filename="package://twil_description/meshes/caster_support.stl" />
</geometry>
</collision>
</link>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:macro name="castor_wheel" params="name parent *origin">
+ <xacro:macro name="caster_wheel" params="name parent *origin">
<link name="${name}">
<inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package://twil_description/meshes/castor_wheel.stl" />
+ <mesh filename="package://twil_description/meshes/caster_wheel.stl" />
</geometry>
<material name="">
<color rgba="0.75294 0.75294 0.75294 1" />
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
- <mesh filename="package://twil_description/meshes/castor_wheel.stl" />
+ <mesh filename="package://twil_description/meshes/caster_wheel.stl" />
</geometry>
</collision>
</link>
<?xml version="1.0"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="twil">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="twil">
<property name="M_PI" value="3.1415926535897931" />
<xacro:include filename="$(find twil_description)/xacro/chassis.urdf.xacro" />
<xacro:include filename="$(find twil_description)/xacro/fixed_wheel_support.urdf.xacro" />
<xacro:include filename="$(find twil_description)/xacro/fixed_wheel.urdf.xacro" />
- <xacro:include filename="$(find twil_description)/xacro/castor_support.urdf.xacro" />
- <xacro:include filename="$(find twil_description)/xacro/castor_base.urdf.xacro" />
- <xacro:include filename="$(find twil_description)/xacro/castor_wheel.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/caster_support.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/caster_base.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/caster_wheel.urdf.xacro" />
<xacro:include filename="$(find twil_description)/xacro/battery_bosch_12v.urdf.xacro" />
<xacro:include filename="$(find twil_description)/xacro/cpu.urdf.xacro" />
<xacro:include filename="$(find twil_description)/xacro/eurocard.urdf.xacro" />
<origin xyz="0 0.0 -0.095" rpy="0 0 0" />
</xacro:fixed_wheel>
- <xacro:castor_support name="castor_support" parent="chassis">
+ <xacro:caster_support name="caster_support" parent="chassis">
<origin xyz="-0.2 0 -0.002" rpy="0 0 0" />
- </xacro:castor_support>
+ </xacro:caster_support>
- <xacro:castor_base name="castor_base" parent="castor_support">
+ <xacro:caster_base name="caster_base" parent="caster_support">
<origin xyz="0 0 -0.1325" rpy="0 0 0" />
- </xacro:castor_base>
+ </xacro:caster_base>
- <xacro:castor_wheel name="castor_wheel" parent="castor_base">
+ <xacro:caster_wheel name="caster_wheel" parent="caster_base">
<origin xyz="-0.04 0 0" rpy="0 0 0" />
- </xacro:castor_wheel>
+ </xacro:caster_wheel>
- <xacro:castor_support name="front_castor_support" parent="chassis">
+ <xacro:caster_support name="front_caster_support" parent="chassis">
<origin xyz="0.2 0 -0.002" rpy="0 0 0" />
- </xacro:castor_support>
+ </xacro:caster_support>
- <xacro:castor_base name="front_castor_base" parent="front_castor_support">
+ <xacro:caster_base name="front_caster_base" parent="front_caster_support">
<origin xyz="0 0 -0.1325" rpy="0 0 0" />
- </xacro:castor_base>
+ </xacro:caster_base>
- <xacro:castor_wheel name="front_castor_wheel" parent="front_castor_base">
+ <xacro:caster_wheel name="front_caster_wheel" parent="front_caster_base">
<origin xyz="-0.04 0 0" rpy="0 0 0" />
- </xacro:castor_wheel>
+ </xacro:caster_wheel>
<!--xacro:battery_bosch_12v name="battery" parent="chassis">
<origin xyz="-0.15 0.0 ${0.0015+0.0875}" rpy="0.0 0.0 ${M_PI/2}" />
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
- <robotNamespace>/twil</robotNamespace>
+ <!--robotNamespace>/twil</robotNamespace-->
<!-- Custom plugin -->
<!-- robotSimType>twil_gazebo_ros_control/TwilRobotHWSim</robotSimType -->
<?xml version="1.0"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="twil">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="twil">
<xacro:include filename="$(find twil_description)/xacro/twil.urdf.xacro" />
</joint>
- <xacro:include filename="$(find twil_description)/xacro/castor_support.urdf.xacro" />
+ <xacro:include filename="$(find twil_description)/xacro/caster_support.urdf.xacro" />
- <xacro:castor_support name="castor_support_front" parent="chassis">
+ <xacro:caster_support name="caster_support_front" parent="chassis">
<origin xyz="0.2 0 -0.002" rpy="0 0 0" />
- </xacro:castor_support>
+ </xacro:caster_support>
- <xacro:castor_base name="castor_base_front" parent="castor_support_front">
+ <xacro:caster_base name="caster_base_front" parent="caster_support_front">
<origin xyz="0 0 -0.1325" rpy="0 0 0" />
- </xacro:castor_base>
+ </xacro:caster_base>
- <xacro:castor_wheel name="castor_wheel_front" parent="castor_base_front">
+ <xacro:caster_wheel name="caster_wheel_front" parent="caster_base_front">
<origin xyz="-0.04 0 0" rpy="0 0 0" />
- </xacro:castor_wheel>
+ </xacro:caster_wheel>
</robot>
cmake_minimum_required(VERSION 2.8.3)
-project(twil_trajectories)
+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 COMPONENTS roscpp)
+find_package(catkin REQUIRED COMPONENTS roscpp kdl_parser)
find_package(cmake_modules REQUIRED)
## 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 ${PROJECT_NAME}
+# INCLUDE_DIRS include
+# LIBRARIES twil_ident
# CATKIN_DEPENDS other_catkin_pkg
DEPENDS eigen
)
## Specify additional locations of header files
## Your package locations should be listed before other locations
-# include_directories(include)
+# include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
# TODO: Check names of system library include directories (eigen)
-include_directories(
- include ${catkin_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS}
)
## Declare a cpp library
-add_library(${PROJECT_NAME}
- src/circle_path.cpp
- src/eight_path.cpp
-)
+# add_library(twil_ident
+# src/${PROJECT_NAME}/twil_ident.cpp
+# )
## Declare a cpp executable
-add_executable(eight_trajectory src/eight_trajectory.cpp)
-add_executable(pose2d_stamp src/pose2d_stamp.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_trajectories_node twil_trajectories_generate_messages_cpp)
+# add_dependencies(twil_ident_node twil_ident_generate_messages_cpp)
## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}
- ${catkin_LIBRARIES}
- ${eigen_LIBRARIES}
-)
-
-target_link_libraries(eight_trajectory
+target_link_libraries(ident
${catkin_LIBRARIES}
${eigen_LIBRARIES}
- ${PROJECT_NAME}
-)
-
-target_link_libraries(pose2d_stamp
- ${catkin_LIBRARIES}
)
#############
# )
## Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME}
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+install(TARGETS ident
+# 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}
+# 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
#############
## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_trajectories.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_ident.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
<arg name="paused" default="true"/>
<arg name="headless" default="false"/>
<arg name="use_sim_time" default="true"/>
- <arg name="wam" default="false"/>
- <remap from="/twil/joint_states" to="/joint_states" />
+ <remap from="/left_wheel_joint_effort_controller/command" to="/left_wheel_command"/>
+ <remap from="/right_wheel_joint_effort_controller/command" to="/right_wheel_command"/>
+
+ <remap from="/ident/left_wheel_command" to="/left_wheel_command"/>
+ <remap from="/ident/right_wheel_command" to="/right_wheel_command"/>
<include file="$(find twil_description)/launch/gazebo.launch" >
<arg name="paused" value="$(arg paused)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="wam" value="$(arg wam)"/>
</include>
- <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
-
+ <include file="$(find twil_ident)/launch/ident.launch"/>
</launch>
--- /dev/null
+<launch>
+ <include file="$(find twil_controllers)/launch/joint_effort.launch" />
+
+ <node name="ident" pkg="twil_ident" type="ident" output="screen" />
+</launch>
<?xml version="1.0"?>
<package>
- <name>twil_trajectories</name>
+ <name>twil_ident</name>
<version>2.0.0</version>
- <description>The twil_trajectories package</description>
+ <description>The twil_ident package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>eigen</build_depend>
- <build_depend>geometry_msgs</build_depend>
-
+ <build_depend>kdl_parser</build_depend>
<run_depend>eigen</run_depend>
- <run_depend>geometry_msgs</run_depend>
+ <run_depend>kdl_parser</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
--- /dev/null
+/******************************************************************************
+ ROS twil_indet Package
+ Twil Dynamics Model
+ Copyright (C) 2014..2017 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <ros/ros.h>
+
+#include <Eigen/Dense>
+
+#include <std_msgs/Float64.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <sensor_msgs/JointState.h>
+
+#include <kdl_parser/kdl_parser.hpp>
+
+
+class Prbs
+{
+ int index;
+ unsigned int nd;
+ unsigned char *sh;
+
+ public:
+
+ Prbs(unsigned int n=10,unsigned int seed=0);
+ ~Prbs(void);
+
+ void seed(unsigned int s) { index=s % nd; };
+
+ operator int(void);
+};
+
+
+Prbs::Prbs(unsigned int n,unsigned int seed)
+{
+ nd=n;
+ index=seed % nd;
+ sh=new unsigned char[nd];
+ for(unsigned int i=0; i < nd;i++) sh[i]=1;
+ for(unsigned int i=0; i < 98;i++) (int) *this; // call operator int() to exercise
+
+}
+
+Prbs::~Prbs(void)
+{
+// delete[] sh;
+}
+
+Prbs::operator int(void)
+{
+ unsigned char s=sh[nd-1]+sh[nd-2];
+ if(s > 1) s=0;
+ for(int j=nd-2;j >= 0;j--) sh[j+1]=sh[j];
+ sh[0]=s;
+ return sh[index];
+}
+
+
+class Ident
+{
+ public:
+ Ident(ros::NodeHandle node);
+ ~Ident(void);
+ void setCommand(void);
+
+ private:
+ ros::NodeHandle node_;
+
+ ros::Subscriber jointStatesSubscriber;
+ ros::Publisher dynParamPublisher;
+ ros::Publisher leftWheelCommandPublisher;
+ ros::Publisher rightWheelCommandPublisher;
+
+ const int nJoints;
+
+ Eigen::VectorXd u;
+ Eigen::VectorXd thetaEst1;
+ Eigen::VectorXd thetaEst2;
+ Eigen::MatrixXd P1;
+ Eigen::MatrixXd P2;
+
+ std::vector<Prbs> prbs;
+ int iter;
+
+ ros::Time lastTime;
+
+ std::vector<double> wheelRadius;
+ double wheelBase;
+
+ void jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates);
+ void resetCovariance(void);
+};
+
+
+Ident::Ident(ros::NodeHandle node):
+ nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints),wheelRadius(nJoints)
+{
+ node_=node;
+
+ std::string robot_desc_string;
+ if(!node_.getParam("/robot_description",robot_desc_string))
+ {
+ ROS_ERROR("Could not find '/robot_description'.");
+ }
+
+ KDL::Tree tree;
+ if (!kdl_parser::treeFromString(robot_desc_string,tree))
+ {
+ ROS_ERROR("Failed to construct KDL tree.");
+ }
+
+ // get wheelBase from URDF (actually from KDL tree)
+ KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support");
+ KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip();
+
+ segmentMapIter=tree.getSegment("right_wheel_support");
+ KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip();
+
+ wheelRadius.resize(nJoints);
+ wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3);
+
+ // get wheelRadius from URDF (actually from KDL tree)
+ segmentMapIter=tree.getSegment("chassis");
+ KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip();
+
+ segmentMapIter=tree.getSegment("left_wheel");
+ KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint();
+ wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z();
+
+ segmentMapIter=tree.getSegment("right_wheel");
+ KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint();
+ wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z();
+
+ jointStatesSubscriber=node_.subscribe("joint_states",1000,&Ident::jointStatesCB,this);
+ dynParamPublisher=node_.advertise<std_msgs::Float64MultiArray>("ident/dynamic_parameters",1000);
+ leftWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/left_wheel_command",1000);
+ rightWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/right_wheel_command",1000);
+
+ u.setZero();
+ thetaEst1.setZero();
+ thetaEst2.setZero();
+ resetCovariance();
+
+ lastTime=ros::Time::now();
+}
+
+Ident::~Ident(void)
+{
+ jointStatesSubscriber.shutdown();
+}
+
+void Ident::resetCovariance(void)
+{
+ P1.setIdentity();
+ P1*=1;
+ P2.setIdentity();
+ P2*=1;
+ iter=0;
+}
+
+void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates)
+{
+ ros::Duration dt=jointStates->header.stamp-lastTime;
+ lastTime=jointStates->header.stamp;
+
+ Eigen::VectorXd y=-u; //y(k+1)=(u(k+1)-u(k))/dt
+
+ Eigen::VectorXd Phi1(nJoints);
+ Eigen::VectorXd Phi2(nJoints);
+ Phi1[0]=u[1]*u[1]; // u2^2(k)
+ Phi2[0]=u[0]*u[1]; // u1(k)*u2(k)
+
+ Eigen::VectorXd torque(nJoints);
+
+ // u(k+1)
+ // jointStates->velocity[0] is left wheel
+ // jointStates->velocity[1] is right wheel
+ u[0]=(jointStates->velocity[0]*wheelRadius[0]+jointStates->velocity[1]*wheelRadius[1])/2;
+ u[1]=(jointStates->velocity[1]*wheelRadius[1]-jointStates->velocity[0]*wheelRadius[0])/wheelBase;
+
+ for(int i=0;i < nJoints;i++)
+ torque[i]=jointStates->effort[i]; // torque(k)
+
+ y+=u;
+ y/=dt.toSec();
+
+ Phi1[1]=torque[0]+torque[1];
+ Phi2[1]=torque[0]-torque[1];
+
+ double yEst1=Phi1.transpose()*thetaEst1;
+ Eigen::VectorXd K1=P1*Phi1/(1+Phi1.transpose()*P1*Phi1);
+ thetaEst1+=K1*(y[0]-yEst1);
+ P1-=K1*Phi1.transpose()*P1;
+
+ double yEst2=Phi2.transpose()*thetaEst2;
+ Eigen::VectorXd K2=P2*Phi2/(1+Phi2.transpose()*P2*Phi2);
+ thetaEst2+=K2*(y[1]-yEst2);
+ P2-=K2*Phi2.transpose()*P2;
+
+ std_msgs::Float64MultiArray dynParam;
+ dynParam.layout.dim.push_back(std_msgs::MultiArrayDimension());
+ dynParam.layout.dim[0].label="K5 K6 K7 K8 P55 P66 P77 P88";
+ dynParam.layout.dim[0].size=nJoints*4;
+ dynParam.layout.dim[0].stride=1;
+ dynParam.layout.data_offset=0;
+ for(int i=0;i < nJoints;i++)
+ {
+ dynParam.data.push_back(thetaEst1[i]);
+ dynParam.data.push_back(thetaEst2[i]);
+ }
+ for(int i=0;i < nJoints;i++)
+ {
+ dynParam.data.push_back(P1(i,i));
+ dynParam.data.push_back(P2(i,i));
+ }
+ dynParamPublisher.publish(dynParam);
+
+// if(iter++ > 2048) resetCovariance();
+}
+
+void Ident::setCommand(void)
+{
+ std_msgs::Float64 leftCommand;
+ std_msgs::Float64 rightCommand;
+ leftCommand.data=10.0*prbs[0]-5.0;
+ rightCommand.data=10.0*prbs[1]-5.0;
+ leftWheelCommandPublisher.publish(leftCommand);
+ rightWheelCommandPublisher.publish(rightCommand);
+}
+
+int main(int argc,char* argv[])
+{
+ ros::init(argc,argv,"twil_ident");
+ ros::NodeHandle node;
+
+ Ident ident(node);
+
+ ros::Rate loop(100);
+ while(ros::ok())
+ {
+ ident.setCommand();
+
+ ros::spinOnce();
+ loop.sleep();
+ }
+ return 0;
+}
+++ /dev/null
-#ifndef CIRCLE_PATH_H
-#define CIRCLE_PATH_H
-
-#include <Eigen/Dense>
-
-/** Circle path
-* @author Walter Fetter Lages <w.fetter@ieee.org>
-*/
-class CirclePath
-{
- Eigen::Vector2d pc_;
- double phi0_;
- double r_;
- double w_;
-
- public:
-
- /** Build a circle path.
- * @param pc circle center point.
- * @param phi0 initial phase.
- * @param r circle radius.
- * @param w angular velocity.
- */
- CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w);
-
- /** Build a circle path.
- * @param p0 starting pose.
- * @param r circle radius.
- * @param w angular velocity.
- */
- CirclePath(const Eigen::Vector3d &p0,double r,double w);
-
- /** Build a circle path.
- * @param pc circle center point.
- * @param p0 starting point.
- * @param w angular velocity.
- */
- CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w);
-
- /** Destroy a circle path.
- */
- ~CirclePath(void) { };
-
- /** Get path point.
- * @param t path time.
- * @return path point.
- */
- Eigen::Vector3d point(double t) const;
-
- /** Get path steering controls.
- * @param t path time.
- * @return steering controls.
- */
- Eigen::Vector2d steering(double t) const;
-};
-#endif
+++ /dev/null
-#ifndef EIGHT_PATH_H
-#define EIGHT_PATH_H
-
-#include <twil_trajectories/circle_path.h>
-
-/** 8 path
-* @author Walter Fetter Lages <w.fetter@ieee.org>
-*/
-class EightPath
-{
- CirclePath c1_;
- CirclePath c2_;
- double period_;
-
- public:
-
- /** Build an 8 path.
- * @param pc center point.
- * @param r radius.
- * @param w angular velocity.
- */
- EightPath(const Eigen::Vector2d &pc,double r,double w);
-
- /** Destroy an 8 path.
- */
- ~EightPath(void) { };
-
- /** Get path point.
- * @param t path time.
- * @return path point.
- */
- Eigen::Vector3d point(double t) const;
-
- /** Get path steering controls.
- * @param t path time.
- * @return steering controls.
- */
- Eigen::Vector2d steering(double t) const;
-};
-#endif
+++ /dev/null
-#include <twil_trajectories/circle_path.h>
-
-#define sqr(x) (x*x)
-#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x)))
-
-CirclePath::CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w)
-{
- pc_=pc;
- phi0_=phi0;
- r_=r;
- w_=w;
-}
-
-CirclePath::CirclePath(const Eigen::Vector3d &p0,double r,double w)
-{
- phi0_=p0[2]-sgn(w)*M_PI_2;
- pc_[0]=p0[0]-r*cos(phi0_);
- pc_[1]=p0[1]-r*sin(phi0_);
- r_=r;
- w_=w;
-}
-
-CirclePath::CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w)
-{
- pc_=pc;
- w_=w;
- phi0_=atan2(p0[1]-pc[1],p0[0]-pc[0]);
- r_=sqrt(sqr(p0[1]-pc[1])+sqr(p0[0]-pc[0]));
-}
-
-Eigen::Vector3d CirclePath::point(double t) const
-{
- double wt=w_*t;
- Eigen::Vector3d p;
-
- p[0]=pc_[0]+r_*cos(wt+phi0_);
- p[1]=pc_[1]+r_*sin(wt+phi0_);
- p[2]=wt+phi0_+sgn(w_)*M_PI_2;
-
- return p;
-}
-
-Eigen::Vector2d CirclePath::steering(double t) const
-{
- Eigen::Vector2d e(2);
-
- e[0]=w_*r_;
- e[1]=w_;
-
- return e;
-}
+++ /dev/null
-#include <twil_trajectories/eight_path.h>
-
-#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x)))
-
-EightPath::EightPath(const Eigen::Vector2d &pc,double r,double w):
-c1_(Eigen::Vector2d(pc[0],pc[1]+r),-M_PI_2*sgn(w),r,w),
-c2_(Eigen::Vector2d(pc[0],pc[1]-r),2.5*M_PI*sgn(w),r,-w)
-{
- period_=4*M_PI/w;
-};
-
-Eigen::Vector3d EightPath::point(double t) const
-{
- double tc=fmod(t,period_/2.0);
-
- if(int(t/(period_/2.0)) % 2 == 0) return c1_.point(tc);
- else return c2_.point(tc);
-}
-
-Eigen::Vector2d EightPath::steering(double t) const
-{
- double tc=fmod(t,period_/2.0);
-
- if(int(t/(period_/2.0)) % 2 == 0) return c1_.steering(tc);
- else return c2_.steering(tc);
-}
+++ /dev/null
-#include <ros/ros.h>
-#include <geometry_msgs/Pose2D.h>
-
-#include <twil_trajectories/eight_path.h>
-
-class EightTrajectory
-{
- public:
- EightTrajectory(ros::NodeHandle node);
- ~EightTrajectory(void);
- void setCommand(ros::Duration t);
-
- private:
- ros::NodeHandle node_;
- const EightPath *path;
-
- ros::Publisher commandPublisher;
-};
-
-
-EightTrajectory::EightTrajectory(ros::NodeHandle node)
-{
- node_=node;
- Eigen::Vector2d pc;
- double w;
- double r;
-
- ros::param::get("~x",pc[0]);
- ros::param::get("~y",pc[1]);
- ros::param::get("~radius",r);
- ros::param::get("~ang_vel",w);
-
- path=new EightPath(pc,r,w);
- commandPublisher=node_.advertise<geometry_msgs::Pose2D>("command",1000);
-}
-
-EightTrajectory::~EightTrajectory(void)
-{
- commandPublisher.shutdown();
- delete path;
-}
-
-void EightTrajectory::setCommand(ros::Duration t)
-{
- Eigen::Vector3d p=path->point(t.toSec());
-
- geometry_msgs::Pose2D command;
- command.x=p[0];
- command.y=p[1];
- command.theta=p[2];
- commandPublisher.publish(command);
-}
-
-int main(int argc,char* argv[])
-{
-
- ros::init(argc,argv,"eight_trajectory");
- ros::NodeHandle node;
-
- EightTrajectory eightTrajectory(node);
-
- ros::Rate loop(100);
-
- ros::Time t0=ros::Time::now();
- while(ros::ok())
- {
- eightTrajectory.setCommand(ros::Time::now()-t0);
-
- ros::spinOnce();
- loop.sleep();
- }
- return 0;
-}
+++ /dev/null
-#include <ros/ros.h>
-
-#include <Eigen/Dense>
-
-#include <geometry_msgs/Pose2D.h>
-#include <geometry_msgs/PoseStamped.h>
-
-class Pose2DStamp
-{
- public:
- Pose2DStamp(ros::NodeHandle node);
- ~Pose2DStamp(void);
-
- private:
- ros::NodeHandle node_;
-
- ros::Subscriber poseSubscriber;
- ros::Publisher posePublisher;
-
- int seq;
- std::string frame_id;
-
- void poseCB(const geometry_msgs::Pose2D::ConstPtr &pose);
-};
-
-
-Pose2DStamp::Pose2DStamp(ros::NodeHandle node)
-{
- node_=node;
-
- poseSubscriber=node_.subscribe("command",1000,&Pose2DStamp::poseCB,this);
- posePublisher=node_.advertise<geometry_msgs::PoseStamped>("command_stamped",1000);
-
- seq=0;
-
- ros::param::get("~frame_id",frame_id);
-}
-
-Pose2DStamp::~Pose2DStamp(void)
-{
- poseSubscriber.shutdown();
- posePublisher.shutdown();
-}
-
-void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose)
-{
- geometry_msgs::PoseStamped stamped;
- stamped.header.stamp=ros::Time::now();
- stamped.header.frame_id=frame_id;
- stamped.pose.position.x=pose->x;
- stamped.pose.position.y=pose->y;
- stamped.pose.position.z=0;
- stamped.pose.orientation.x=0;
- stamped.pose.orientation.y=0;
- stamped.pose.orientation.z=sin(pose->theta/2.0);
- stamped.pose.orientation.w=cos(pose->theta/2.0);
- posePublisher.publish(stamped);
-}
-
-int main(int argc,char* argv[])
-{
- ros::init(argc,argv,"pose2d_stamp");
- ros::NodeHandle node;
-
- Pose2DStamp pose2DStamp(node);
-
- ros::Rate loop(100);
- ros::Time t0=ros::Time::now();
- while(ros::ok())
- {
- ros::spinOnce();
- loop.sleep();
- }
- return 0;
-}
projects / twil.git / commitdiff