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

Added two variables to enable/disable the simulation of the IR sensor feet.

Implemented the topic publishers with IR sensor feet data.
parent b01c5cb7
No related branches found
No related tags found
1 merge request!8Kinetic migration
...@@ -5,6 +5,8 @@ darwin: ...@@ -5,6 +5,8 @@ darwin:
publish_rate: 50 publish_rate: 50
darwin_controller: darwin_controller:
left_sensor_foot_present: true
right_sensor_foot_present: true
type: effort_controllers/DarwinController type: effort_controllers/DarwinController
joints: joints:
- j_shoulder_pitch_r - j_shoulder_pitch_r
......
...@@ -45,6 +45,7 @@ ...@@ -45,6 +45,7 @@
#include <ros/node_handle.h> #include <ros/node_handle.h>
#include <std_msgs/Int8.h> #include <std_msgs/Int8.h>
#include <sensor_msgs/Imu.h> #include <sensor_msgs/Imu.h>
#include "sensor_msgs/Range.h"
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <trajectory_msgs/JointTrajectoryPoint.h> #include <trajectory_msgs/JointTrajectoryPoint.h>
#include <control_msgs/JointTrajectoryAction.h> #include <control_msgs/JointTrajectoryAction.h>
...@@ -73,6 +74,7 @@ ...@@ -73,6 +74,7 @@
#include <humanoid_common_msgs/get_smart_charger_config.h> #include <humanoid_common_msgs/get_smart_charger_config.h>
#include <humanoid_common_msgs/set_smart_charger_config.h> #include <humanoid_common_msgs/set_smart_charger_config.h>
#include <humanoid_common_msgs/smart_charger_data.h> #include <humanoid_common_msgs/smart_charger_data.h>
#include <humanoid_common_msgs/ir_foot_data.h>
#include <humanoid_modules/humanoid_module.h> #include <humanoid_modules/humanoid_module.h>
#include <gazebo/transport/transport.hh> #include <gazebo/transport/transport.hh>
...@@ -197,8 +199,20 @@ namespace darwin_controller ...@@ -197,8 +199,20 @@ namespace darwin_controller
ros::Subscriber leds_subscriber_; ros::Subscriber leds_subscriber_;
void leds_callback(const humanoid_common_msgs::leds::ConstPtr& msg); void leds_callback(const humanoid_common_msgs::leds::ConstPtr& msg);
ros::Subscriber range_sub;
void range_callback(const sensor_msgs::Range::ConstPtr& msg);
CMutex walk_access; CMutex walk_access;
// feet sensors publihsers
ros::Publisher left_foot_sensor_data_publisher_;
humanoid_common_msgs::ir_foot_data left_foot_sensor_data_msg_;
bool left_sensor_foot_present;
ros::Publisher right_foot_sensor_data_publisher_;
humanoid_common_msgs::ir_foot_data right_foot_sensor_data_msg_;
bool right_sensor_foot_present;
// fall detection publisher // fall detection publisher
ros::Publisher fallen_state_publisher_; ros::Publisher fallen_state_publisher_;
std_msgs::Int8 fallen_state_msg_; std_msgs::Int8 fallen_state_msg_;
......
...@@ -273,6 +273,50 @@ namespace darwin_controller ...@@ -273,6 +273,50 @@ namespace darwin_controller
// initialize publisher // initialize publisher
this->fallen_state_publisher_ = root_nh.advertise<std_msgs::Int8>("robot/fallen_state", 1); this->fallen_state_publisher_ = root_nh.advertise<std_msgs::Int8>("robot/fallen_state", 1);
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);
if(this->left_sensor_foot_present)
{
this->left_foot_sensor_data_publisher_ = root_nh.advertise<humanoid_common_msgs::ir_foot_data>("sensors/left_foot_data",1);
this->left_foot_sensor_data_msg_.names.resize(10);
this->left_foot_sensor_data_msg_.names[0]="down_left_middle";
this->left_foot_sensor_data_msg_.names[1]="down_left_rear";
this->left_foot_sensor_data_msg_.names[2]="down_analog";
this->left_foot_sensor_data_msg_.names[3]="down_left_front";
this->left_foot_sensor_data_msg_.names[4]="down_right_rear";
this->left_foot_sensor_data_msg_.names[5]="down_right_middle";
this->left_foot_sensor_data_msg_.names[6]="down_right_front";
this->left_foot_sensor_data_msg_.names[7]="front_left";
this->left_foot_sensor_data_msg_.names[8]="front_right";
this->left_foot_sensor_data_msg_.names[9]="front_analog";
this->left_foot_sensor_data_msg_.voltages.resize(10);
this->left_foot_sensor_data_msg_.status.resize(10);
}
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_foot_sensor_data_publisher_ = root_nh.advertise<humanoid_common_msgs::ir_foot_data>("sensors/right_foot_data",1);
this->right_foot_sensor_data_msg_.names.resize(10);
this->right_foot_sensor_data_msg_.names[0]="down_left_middle";
this->right_foot_sensor_data_msg_.names[1]="down_left_rear";
this->right_foot_sensor_data_msg_.names[2]="down_analog";
this->right_foot_sensor_data_msg_.names[3]="down_left_front";
this->right_foot_sensor_data_msg_.names[4]="down_right_rear";
this->right_foot_sensor_data_msg_.names[5]="down_right_middle";
this->right_foot_sensor_data_msg_.names[6]="down_right_front";
this->right_foot_sensor_data_msg_.names[7]="front_left";
this->right_foot_sensor_data_msg_.names[8]="front_right";
this->right_foot_sensor_data_msg_.names[9]="front_analog";
this->right_foot_sensor_data_msg_.voltages.resize(10);
this->right_foot_sensor_data_msg_.status.resize(10);
}
if(this->right_sensor_foot_present || this->right_sensor_foot_present)
this->range_sub=root_nh.subscribe("sensors/range", 1, &DarwinController<HardwareInterface>::range_callback,this);
// Controller name // Controller name
name_=getLeafNamespace(controller_nh_); name_=getLeafNamespace(controller_nh_);
...@@ -1075,7 +1119,6 @@ namespace darwin_controller ...@@ -1075,7 +1119,6 @@ namespace darwin_controller
} }
/************************************ get_tilt_pid *****************************************/ /************************************ get_tilt_pid *****************************************/
/****************************************** imu *************************************************/
/****************************************** imu *************************************************/ /****************************************** imu *************************************************/
template <class HardwareInterface> template <class HardwareInterface>
void DarwinController<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg) void DarwinController<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg)
...@@ -1106,6 +1149,137 @@ namespace darwin_controller ...@@ -1106,6 +1149,137 @@ namespace darwin_controller
} }
/****************************************** imu *************************************************/ /****************************************** imu *************************************************/
template <class HardwareInterface>
void DarwinController<HardwareInterface>::range_callback(const sensor_msgs::Range::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(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_);
}
}
}
/****************************************** cmd_vel *********************************************/ /****************************************** cmd_vel *********************************************/
template <class HardwareInterface> template <class HardwareInterface>
void DarwinController<HardwareInterface>::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg) void DarwinController<HardwareInterface>::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment