Skip to content
Snippets Groups Projects
Commit e1a25da0 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the BNO055 Simulated IMU to the Gazebo plugin. 

Removed the smart charger and feet contacts modules.
parent 06d0ccc0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 3
− 3
View file @ e1a25da0
......@@ -11,7 +11,7 @@ find_package(catkin REQUIRED roscpp urdf controller_interface hardware_interface
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
find_package(gazebo REQUIRED)
FIND_PACKAGE(bno055_imu_sim REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
......@@ -110,7 +110,7 @@ include_directories(${STM32_HAL_PATH}/f1/include/core)
include_directories(${iriutils_INCLUDE_DIR})
include_directories(${comm_INCLUDE_DIR})
include_directories(${dynamixel_INCLUDE_DIR})
include_directories(${GAZEBO_INCLUDE_DIRS})
include_directories(${bno055_imu_sim_INCLUDE_DIR})
add_definitions(-DSTM32F103xE)
add_definitions(-DUSE_HAL_DRIVER)
......@@ -164,7 +164,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${comm_LIBRARY})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${dynamixel_LIBRARY})
target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${bno055_imu_sim_LIBRARY})
#############
## Install ##
......
include/darwin_controller_cpp.h
+ 6
− 29
View file @ e1a25da0
......@@ -52,14 +52,11 @@
#include <hardware_interface/joint_command_interface.h>
#include <control_toolbox/pid.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <tf/transform_datatypes.h>
#include "mutex.h"
#include "dynamixel_slave_serial.h"
#include "bno055_imu_sim.h"
namespace darwin_controller_cpp
{
......@@ -84,6 +81,8 @@ namespace darwin_controller_cpp
typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
CDynamixelSlaveSerial *dyn_slave;
CBNO055IMUSim *imu_sim;
std::string name_; ///< Controller name.
std::vector<JointHandle> joints_; ///< Handles to controlled joints.
......@@ -100,32 +99,10 @@ namespace darwin_controller_cpp
ros::Subscriber accel_sub;
void accel_callback(const sensor_msgs::Imu::ConstPtr& msg);
ros::Subscriber range_sub;
void range_callback(const sensor_msgs::Range::ConstPtr& msg);
ros::Subscriber imu_sub;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
CMutex walk_access;
// feet sensors
bool left_sensor_foot_present;
bool right_sensor_foot_present;
// smart charger attributes
// internal attributes
bool use_smart_charger;
double initial_charge;
double current_charge;
double discharge_rate;
double charge_current;
unsigned short int average_time_to_full;
unsigned short int average_time_to_empty;
std::string charge_station_link;
std::string charge_robot_link;
bool charger_connected;
bool charger_enabled;
// gazebo interface for contacts
gazebo::transport::NodePtr gazebo_node;
gazebo::transport::SubscriberPtr contacts_subs;
void OnContacts(ConstContactsPtr &msg);
};
} // namespace
......
include/darwin_controller_cpp_impl.h
+ 39
− 228
View file @ e1a25da0
......@@ -157,7 +157,8 @@ namespace darwin_controller_cpp
template <class HardwareInterface>
DarwinControllerCPP<HardwareInterface>::DarwinControllerCPP()
{
this->dyn_slave=NULL;
this->dyn_slave=NULL;
this->imu_sim=NULL;
}
template <class HardwareInterface>
......@@ -169,53 +170,10 @@ namespace darwin_controller_cpp
// Cache controller node handle
controller_nh_ = controller_nh;
// smart charger interface
this->use_smart_charger=DEFAULT_ENABLE_SMART_CHARGER;
controller_nh_.getParam("use_smart_charger", this->use_smart_charger);
ROS_DEBUG_STREAM_NAMED(name_, "Smart charger is: " << this->use_smart_charger);
if(this->use_smart_charger)
{
// initialize gazebo interfaces
this->gazebo_node=gazebo::transport::NodePtr(new gazebo::transport::Node());
this->gazebo_node->Init();
this->contacts_subs=this->gazebo_node->Subscribe("/gazebo/default/physics/contacts",&DarwinControllerCPP<HardwareInterface>::OnContacts,this);
// read parameters
this->initial_charge=DEFAULT_INITIAL_CHARGE;
controller_nh_.getParam("initial_charge", this->initial_charge);
ROS_DEBUG_STREAM_NAMED(name_, "Initial charge set to: " << this->initial_charge);
this->current_charge=this->initial_charge;
this->discharge_rate=DEFAULT_DISCHARGE_RATE;
controller_nh_.getParam("discharge_rate", this->discharge_rate);
ROS_DEBUG_STREAM_NAMED(name_, "Discharge rate set to: " << this->discharge_rate);
this->charge_current=DEFAULT_CHARGE_CURRENT;
controller_nh_.getParam("charge_current", this->charge_current);
ROS_DEBUG_STREAM_NAMED(name_, "Charge current set to: " << this->charge_current);
this->charge_station_link=DEFAULT_CHARGE_STATION_LINK;
controller_nh_.getParam("charge_station_link", this->charge_station_link);
ROS_DEBUG_STREAM_NAMED(name_, "Charger station link set to: " << this->charge_station_link);
this->charge_robot_link=DEFAULT_CHARGE_ROBOT_LINK;
controller_nh_.getParam("charge_robot_link", this->charge_robot_link);
ROS_DEBUG_STREAM_NAMED(name_, "Charger robot link set to: " << this->charge_robot_link);
this->average_time_to_full=-1;
this->average_time_to_empty=-1;
this->charger_connected=false;
this->charger_enabled=false;
}
// initialize topics
this->gyro_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinControllerCPP<HardwareInterface>::gyro_callback,this);
this->accel_sub=root_nh.subscribe("sensors/raw_accel", 1, &DarwinControllerCPP<HardwareInterface>::accel_callback,this);
this->left_sensor_foot_present=false;
controller_nh_.getParam("left_sensor_foot_present", this->left_sensor_foot_present);
ROS_DEBUG_STREAM_NAMED(name_, "left sensor foot is: " << this->left_sensor_foot_present);
this->right_sensor_foot_present=false;
controller_nh_.getParam("right_sensor_foot_present", this->right_sensor_foot_present);
ROS_DEBUG_STREAM_NAMED(name_, "right sensor foot is: " << this->right_sensor_foot_present);
if(this->right_sensor_foot_present || this->right_sensor_foot_present)
this->range_sub=root_nh.subscribe("sensors/range", 1, &DarwinControllerCPP<HardwareInterface>::range_callback,this);
this->imu_sub=root_nh.subscribe("sensors/raw_imu", 1, &DarwinControllerCPP<HardwareInterface>::imu_callback,this);
// Controller name
name_=getLeafNamespace(controller_nh_);
......@@ -278,10 +236,10 @@ namespace darwin_controller_cpp
/* initialize dynamixel slave */
darwin_dyn_slave_init();
// initialize dynamixel slave server
ROS_INFO("Load slave parameters");
ROS_INFO("Load dynamixel slave parameters");
serial_device="/dev/ttyUSB0";
controller_nh_.getParam("serial_device", serial_device);
ROS_INFO_STREAM("serial_device is: " << serial_device);
controller_nh_.getParam("dyn_serial_device", serial_device);
ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
try{
this->dyn_slave=new CDynamixelSlaveSerial("dyn_slave",serial_device,dyn_version2);
this->dyn_slave->set_baudrate(2000000/(ram_data[DARWIN_BAUD_RATE]+1));
......@@ -296,6 +254,22 @@ namespace darwin_controller_cpp
}
ROS_ERROR_STREAM(e.what());
}
// initialize IMU simulator
ROS_INFO("Load IMU simulator");
serial_device="/dev/ttyUSB0";
controller_nh_.getParam("imu_serial_device", serial_device);
ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
try{
this->imu_sim=new CBNO055IMUSim("darwin_imu",serial_device);
this->imu_sim->set_calibrated();
}catch(CException &e){
if(this->imu_sim!=NULL)
{
delete this->imu_sim;
this->imu_sim=NULL;
}
ROS_ERROR_STREAM(e.what());
}
/* initialize motion modules */
manager_init(7800);// motion manager period in us
ROS_INFO("CDarwinSim : motion manager initialized");
......@@ -337,60 +311,6 @@ namespace darwin_controller_cpp
double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
this->joints_[i].setCommand(command);
}
if(this->use_smart_charger)
{
if(this->charger_enabled)
{
if(this->charger_connected)
{
this->current_charge+=this->charge_current*period.toSec()/3600.0;
if(this->current_charge>this->initial_charge)
this->current_charge=this->initial_charge;
this->average_time_to_full=(uint16_t)(((this->initial_charge-this->current_charge)/this->charge_current)*60.0);
}
else
{
this->current_charge-=this->discharge_rate*period.toSec()/3600.0;
if(this->current_charge<0.0)
{
this->current_charge=0.0;
ROS_ERROR("Battery died out before it could be recharged");
}
this->average_time_to_empty=(uint16_t)((this->current_charge/this->discharge_rate)*60.0);
this->average_time_to_full=(uint16_t)-1;
}
}
}
}
template <class HardwareInterface>
void DarwinControllerCPP<HardwareInterface>::OnContacts(ConstContactsPtr &msg)
{
for(unsigned int i=0;i<msg->contact_size();i++)
{
if(msg->contact(i).collision1().find(this->charge_station_link)!=std::string::npos)
{
if(msg->contact(i).collision2().find(this->charge_robot_link)!=std::string::npos)
{
this->charger_connected=true;
break;
}
else
this->charger_connected=false;
}
else if(msg->contact(i).collision1().find(this->charge_robot_link)!=std::string::npos)
{
if(msg->contact(i).collision2().find(this->charge_station_link)!=std::string::npos)
{
this->charger_connected=true;
break;
}
else
this->charger_connected=false;
}
else
this->charger_connected=false;
}
}
/****************************************** imu *************************************************/
......@@ -421,139 +341,25 @@ namespace darwin_controller_cpp
accel_data[1]=(int32_t)msg->linear_acceleration.y*1000.0;
accel_data[2]=(int32_t)msg->linear_acceleration.z*1000.0;
}
/****************************************** imu *************************************************/
template <class HardwareInterface>
void DarwinControllerCPP<HardwareInterface>::range_callback(const sensor_msgs::Range::ConstPtr& msg)
void DarwinControllerCPP<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
std::string frame_name;
unsigned char threshold;
static unsigned int remaining_left_sensors=10,remaining_right_sensors=10;
if(msg->header.frame_id.find("/")==std::string::npos)
frame_name=msg->header.frame_id;
else
frame_name=msg->header.frame_id.substr(msg->header.frame_id.find("/")+1);
/* handle the feet sensors */
if(msg->range>(msg->max_range/2.0))
threshold=0x01;
else
threshold=0x00;
if(this->left_sensor_foot_present)
if(this->imu_sim!=NULL)
{
if(frame_name=="left_down_right_rear_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[4]=msg->range;
// this->left_foot_sensor_data_msg_.status[4]=threshold;
remaining_left_sensors--;
}
else if(frame_name=="left_down_right_middle_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[5]=msg->range;
// this->left_foot_sensor_data_msg_.status[5]=threshold;
remaining_left_sensors--;
}
else if(frame_name=="left_down_right_front_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[6]=msg->range;
// this->left_foot_sensor_data_msg_.status[6]=threshold;
remaining_left_sensors--;
}
else if(frame_name=="left_down_left_rear_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[1]=msg->range;
// this->left_foot_sensor_data_msg_.status[1]=threshold;
remaining_left_sensors--;
}
else if(frame_name=="left_down_left_middle_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[0]=msg->range;
// this->left_foot_sensor_data_msg_.status[0]=threshold;
remaining_left_sensors--;
}
else if(frame_name=="left_down_left_front_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[3]=msg->range;
// this->left_foot_sensor_data_msg_.status[3]=threshold;
remaining_left_sensors--;
}
else if(frame_name=="left_front_right_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[8]=msg->range;
// this->left_foot_sensor_data_msg_.status[8]=threshold;
remaining_left_sensors--;
}
else if(frame_name=="left_front_left_ir_link")
{
// this->left_foot_sensor_data_msg_.voltages[7]=msg->range;
// this->left_foot_sensor_data_msg_.status[7]=threshold;
remaining_left_sensors--;
}
if(remaining_left_sensors==0)
{
remaining_left_sensors=10;
// this->left_foot_sensor_data_publisher_.publish(this->left_foot_sensor_data_msg_);
}
}
if(this->right_sensor_foot_present)
{
if(frame_name=="right_down_right_rear_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[4]=msg->range;
// this->right_foot_sensor_data_msg_.status[4]=threshold;
remaining_right_sensors--;
}
else if(frame_name=="right_down_right_middle_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[5]=msg->range;
// this->right_foot_sensor_data_msg_.status[5]=threshold;
remaining_right_sensors--;
}
else if(frame_name=="right_down_right_front_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[6]=msg->range;
// this->right_foot_sensor_data_msg_.status[6]=threshold;
remaining_right_sensors--;
}
else if(frame_name=="right_down_left_rear_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[1]=msg->range;
// this->right_foot_sensor_data_msg_.status[1]=threshold;
remaining_right_sensors--;
}
else if(frame_name=="right_down_left_middle_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[0]=msg->range;
// this->right_foot_sensor_data_msg_.status[0]=threshold;
remaining_right_sensors--;
}
else if(frame_name=="right_down_left_front_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[3]=msg->range;
// this->right_foot_sensor_data_msg_.status[3]=threshold;
remaining_right_sensors--;
}
else if(frame_name=="right_front_right_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[8]=msg->range;
// this->right_foot_sensor_data_msg_.status[8]=threshold;
remaining_right_sensors--;
}
else if(frame_name=="right_front_left_ir_link")
{
// this->right_foot_sensor_data_msg_.voltages[7]=msg->range;
// this->right_foot_sensor_data_msg_.status[7]=threshold;
remaining_right_sensors--;
}
if(remaining_right_sensors==0)
{
remaining_right_sensors=10;
// this->right_foot_sensor_data_publisher_.publish(this->right_foot_sensor_data_msg_);
}
this->imu_sim->set_quaternion_data(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
tf::Quaternion q(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
this->imu_sim->set_euler_angles_data(yaw,roll,pitch);
this->imu_sim->set_linear_accel_data(msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);
this->imu_sim->set_gyro_data(msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);
}
}
/****************************************** imu *************************************************/
template <class HardwareInterface>
DarwinControllerCPP<HardwareInterface>::~DarwinControllerCPP()
{
......@@ -562,6 +368,11 @@ namespace darwin_controller_cpp
delete this->dyn_slave;
this->dyn_slave=NULL;
}
if(this->imu_sim!=NULL)
{
delete this->imu_sim;
this->imu_sim=NULL;
}
}
} // namespace
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment