project(gazebo_ros_electrical)
## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
roscpp
transmission_interface
urdf
- voltage_controllers
+ electrical_interface
)
## System dependencies are found with CMake's conventions
catkin_package(
INCLUDE_DIRS include
LIBRARIES gazebo_ros_electrical
-# CATKIN_DEPENDS angles control_toolbox gazebo hardware_interface joint_limits_interface pluginlib roscpp transmission_interface urdf voltage_controllers
+# CATKIN_DEPENDS angles control_toolbox gazebo hardware_interface joint_limits_interface pluginlib roscpp transmission_interface urdf electrical_interface
# DEPENDS system_lib
)
// URDF
#include <urdf/model.h>
-//Fix this
-#include <voltage_controllers/joint_command_interface.h>
+// Electrical inteface
+#include <electrical_interface/joint_command_interface.h>
namespace gazebo_ros_electrical
hardware_interface::EffortJointInterface ej_interface_;
hardware_interface::PositionJointInterface pj_interface_;
hardware_interface::VelocityJointInterface vj_interface_;
- hardware_interface::VoltageJointInterface uj_interface_;
+ electrical_interface::VoltageJointInterface uj_interface_;
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_;
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_;
<build_depend>roscpp</build_depend>
<build_depend>transmission_interface</build_depend>
<build_depend>urdf</build_depend>
- <build_depend>voltage_controllers</build_depend>
+ <build_depend>electrical_interface</build_depend>
<run_depend>angles</run_depend>
<run_depend>control_toolbox</run_depend>
<run_depend>gazebo</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>transmission_interface</run_depend>
<run_depend>urdf</run_depend>
- <run_depend>voltage_controllers</run_depend>
+ <run_depend>electrical_interface</run_depend>
<!-- The export tag contains other, unspecified, tags -->
for(unsigned int j=0; j < n_dof_; j++)
{
// Gazebo has an interesting API...
+#if GAZEBO_MAJOR_VERSION >= 8
+ double position = sim_joints_[j]->Position(0);
+#else
+ double position = sim_joints_[j]->GetAngle(0).Radian();
+#endif
if (joint_types_[j] == urdf::Joint::PRISMATIC)
{
- joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian();
+ joint_position_[j] = position;
}
else
{
joint_position_[j] += angles::shortest_angular_distance(joint_position_[j],
- sim_joints_[j]->GetAngle(0).Radian());
+ position);
}
joint_velocity_[j] = sim_joints_[j]->GetVelocity(0);
joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0));
if (urdf_model != NULL)
{
- const boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(joint_name);
+ const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name);
if (urdf_joint != NULL)
{
*joint_type = urdf_joint->type;