icart_mini_hw_sim.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2014, Daiki Maekawa and Robot Design and Control Lab.
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions
00009  * are met:
00010  *
00011  *  * Redistributions of source code must retain the above copyright
00012  *    notice, this list of conditions and the following disclaimer.
00013  *  * Redistributions in binary form must reproduce the above
00014  *    copyright notice, this list of conditions and the following
00015  *    disclaimer in the documentation and/or other materials provided
00016  *    with the distribution.
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00018  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00019  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00020  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00021  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00023  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00024  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00025  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00026  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00027  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <hardware_interface/joint_command_interface.h>
00032 #include <hardware_interface/robot_hw.h>
00033 
00034 #include <gazebo/common/common.hh>
00035 #include <gazebo/physics/physics.hh>
00036 #include <gazebo/gazebo.hh>
00037 
00038 #include <ros/ros.h>
00039 #include <angles/angles.h>
00040 #include <pluginlib/class_list_macros.h>
00041 #include <std_msgs/Bool.h>
00042 
00043 #include <gazebo_ros_control/robot_hw_sim.h>
00044 
00045 #include <urdf/model.h>
00046 
00047 //#include <safety_interface/safety_interface.h>
00048 
00049 namespace icart_mini_gazebo
00050 {
00051 
00052     class ICartMiniHWSim : public gazebo_ros_control::RobotHWSim
00053     {
00054     private:
00055         static const double max_drive_joint_torque_ = 20.0;
00056         
00057         double cmd_[2];
00058         double pos_[2];
00059         double vel_[2];
00060         double eff_[2];
00061         
00062         gazebo::physics::JointPtr joint_[2];
00063         
00064         hardware_interface::JointStateInterface js_interface_;
00065         hardware_interface::VelocityJointInterface vj_interface_;
00066         //safety_interface::SafetyInterface safety_interface_;
00067 
00068     public:
00069         bool initSim(const std::string& robot_namespace, ros::NodeHandle model_nh, gazebo::physics::ModelPtr parent_model,
00070             const urdf::Model* const urdf_model, std::vector<transmission_interface::TransmissionInfo> transmissions)
00071         {
00072             pos_[0] = 0.0; pos_[1] = 0.0;
00073             vel_[0] = 0.0; vel_[1] = 0.0;
00074             eff_[0] = 0.0; eff_[1] = 0.0;
00075             cmd_[0] = 0.0; cmd_[1] = 0.0;
00076 
00077             std::string joint_namespace = robot_namespace.substr(1); //remove leading slash
00078             
00079             std::cout << "joint_namespace = " << joint_namespace << std::endl;
00080 
00081             joint_[0] = parent_model->GetJoint("right_wheel_hinge");
00082             joint_[1] = parent_model->GetJoint("left_wheel_hinge");
00083 
00084             //joint_[0] = parent_model->GetJoint(joint_namespace + "/right_wheel_hinge");
00085             //joint_[1] = parent_model->GetJoint(joint_namespace + "/left_wheel_hinge");
00086             
00087             js_interface_.registerHandle(
00088                 //hardware_interface::JointStateHandle(joint_namespace + "/right_wheel_hinge", &pos_[0], &vel_[0], &eff_[0]));
00089                 hardware_interface::JointStateHandle("right_wheel_hinge", &pos_[0], &vel_[0], &eff_[0]));
00090             js_interface_.registerHandle(
00091                 //hardware_interface::JointStateHandle(joint_namespace + "/left_wheel_hinge", &pos_[1], &vel_[1], &eff_[1]));
00092                 hardware_interface::JointStateHandle("left_wheel_hinge", &pos_[1], &vel_[1], &eff_[1]));
00093             
00094             vj_interface_.registerHandle(
00095                 //hardware_interface::JointHandle(js_interface_.getHandle(joint_namespace + "/right_wheel_hinge"), &cmd_[0]));
00096                 hardware_interface::JointHandle(js_interface_.getHandle("right_wheel_hinge"), &cmd_[0]));
00097             
00098             vj_interface_.registerHandle(
00099                 //hardware_interface::JointHandle(js_interface_.getHandle(joint_namespace + "/right_wheel_hinge"), &cmd_[1]));
00100                 hardware_interface::JointHandle(js_interface_.getHandle("left_wheel_hinge"), &cmd_[1]));
00101 
00102             registerInterface(&js_interface_);
00103             registerInterface(&vj_interface_);
00104 
00105             //registerInterface(&safety_interface_);
00106             
00107             return true;
00108         }
00109 
00110         void readSim(ros::Time time, ros::Duration period){
00111             for(int i=0; i < 2; i++){
00112                 pos_[i] += angles::shortest_angular_distance(pos_[i], joint_[i]->GetAngle(0).Radian());
00113                 vel_[i] = joint_[i]->GetVelocity(0);
00114                 eff_[i] = joint_[i]->GetForce((unsigned int)(0));
00115             }
00116         }
00117 
00118         void writeSim(ros::Time time, ros::Duration period){
00119             for(int i=0; i < 2; i++){
00120                 joint_[i]->SetVelocity(0, cmd_[i]);
00121                 joint_[i]->SetMaxForce(0, max_drive_joint_torque_);
00122             }
00123             
00124             /*
00125             if(safety_interface_.get_state() == safety_interface::safety_state::OK){
00126                 for(int i=0; i < 2; i++){
00127                     joint_[i]->SetVelocity(0, cmd_[i]);
00128                     joint_[i]->SetMaxForce(0, max_drive_joint_torque_);
00129                 }
00130             }else{
00131                 for(int i=0; i < 2; i++) joint_[i]->SetVelocity(0, 0);
00132             }
00133             */
00134         }
00135     };
00136 
00137     typedef boost::shared_ptr<ICartMiniHWSim> ICartMiniHWSimPtr;
00138 }
00139 
00140 PLUGINLIB_EXPORT_CLASS(icart_mini_gazebo::ICartMiniHWSim, gazebo_ros_control::RobotHWSim)
00141 


icart_mini_gazebo
Author(s): Daiki Maekawa
autogenerated on Thu Jun 6 2019 21:06:19