L1.5/rosaria
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Deleted old version of subscribing and publishing and added new implementations. System ready for first tests with user and master plugins

Browse Source
This commit is contained in:
Pioneer3 2018-09-28 17:08:15 +02:00
parent 3efc57bcf5
commit 858cebd7ce
1 changed files with 12 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
RosAria.cpp
View File

@ -56,8 +56,6 @@ public:
void masterstop_cb(const std_msgs::Bool &);
void clutch_cb(const std_msgs::Bool &);
//void cmd_enable_motors_cb();
//void cmd_disable_motors_cb();
void spin();
void publish();
void publish_robot_info();
@ -73,17 +71,12 @@ protected:
ros::Publisher bumpers_pub;
ros::Publisher sonar_pub;
ros::Publisher sonar_pointcloud2_pub;
ros::Publisher voltage_pub;
ros::Publisher recharge_state_pub;
std_msgs::Int8 recharge_state;
ros::Publisher state_of_charge_pub;
ros::Publisher motors_state_pub;
std_msgs::Bool motors_state;
bool published_motors_state;
ros::Subscriber cmdvel_sub;
// Olek new subscribers and publishers
@ -101,16 +94,6 @@ protected:
bool clutch_state;
ros::ServiceServer enable_srv;
ros::ServiceServer disable_srv;
bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
ros::Publisher stop_motors_pub;
ros::ServiceServer stop_motors_srv;
ros::ServiceServer start_motors_srv;
bool stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
//Gripper
ros::ServiceServer gripper_open_srv;
@ -170,10 +153,6 @@ protected:
// dynamic_reconfigure
dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
//Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie
bool stop_robot;
std_msgs::Bool stop_motors_msg;
ros::Time watchdog;
};
@ -375,16 +354,11 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
boost::bind(&RosAriaNode::sonarConnectCb, this),
boost::bind(&RosAriaNode::sonarConnectCb, this));
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
wheel_pub = n.advertise<sensor_msgs::JointState>("wheels",1000);
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
recharge_state.data = -2;
state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
stop_motors_pub = n.advertise<std_msgs::Bool>("stop_motors_state", 5, true /*latch*/ );
motors_state.data = false;
published_motors_state = false;
// subscribe to services
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
@ -398,30 +372,17 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
robot_info_pub = n.advertise<rosaria_msgs::RobotInfoMsg>("robot_info", 1, true /*latch*/ );
// advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
//Damian Gripper
gripper_open_srv = n.advertiseService("gripper_open", &RosAriaNode::gripper_open_cb, this);
gripper_close_srv = n.advertiseService("gripper_close", &RosAriaNode::gripper_close_cb, this);
gripper_up_srv = n.advertiseService("gripper_up", &RosAriaNode::gripper_up_cb, this);
gripper_down_srv = n.advertiseService("gripper_down", &RosAriaNode::gripper_down_cb, this);
//Damian usługi stopu awaryjnego
stop_motors_srv = n.advertiseService("stop_motors", &RosAriaNode::stop_motors_cb, this);
start_motors_srv = n.advertiseService("start_motors", &RosAriaNode::start_motors_cb, this);
stop_robot=false;
stop_motors_msg.data=false;
maxVel=1.4;
maxRot=1;
minDist = 0.7;
veltime = ros::Time::now();
}
RosAriaNode::~RosAriaNode()
@ -558,6 +519,13 @@ int RosAriaNode::Setup()
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// Setting flags to initial values
robot_state = false;
userstop_state = false;
masterstop_state = false;
clutch_state = true;
// Enable the motors
robot->enableMotors();
@ -684,12 +652,6 @@ void RosAriaNode::publish()
bumpers_pub.publish(bumpers);
//Publish battery information
// TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option
std_msgs::Float64 batteryVoltage;
batteryVoltage.data = robot->getRealBatteryVoltageNow();
voltage_pub.publish(batteryVoltage);
if(robot->haveStateOfCharge())
{
std_msgs::Float32 soc;
@ -706,24 +668,6 @@ void RosAriaNode::publish()
recharge_state_pub.publish(recharge_state);
}
// publish motors state if changed
bool e = robot->areMotorsEnabled();
if(e != motors_state.data || !published_motors_state)
{
ROS_INFO("RosAria: publishing stop motors state %d.", e);
motors_state.data = e;
motors_state_pub.publish(motors_state);
published_motors_state = true;
}
//Publikuje czy zatrzymane silniki
if(stop_robot)
{
ROS_INFO("RosAria: publishing new motors state %d.", stop_motors_msg.data);
stop_motors_pub.publish(stop_motors_msg);
stop_robot = false;
}
// Publish sonar information, if enabled.
if (publish_sonar || publish_sonar_pointcloud2)
{
@ -797,60 +741,11 @@ void RosAriaNode::publish()
robot->setRotVel(0);
// ROS_INFO("WATCHDOG %d", WATCHDOG);
}
publish_robot_info();
}
bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
ROS_INFO("RosAria: Enable motors request.");
robot->lock();
if(robot->isEStopPressed())
ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
robot->enableMotors();
robot->unlock();
// todo could wait and see if motors do become enabled, and send a response with an error flag if not
return true;
}
bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
ROS_INFO("RosAria: Disable motors request.");
robot->lock();
robot->disableMotors();
robot->unlock();
// todo could wait and see if motors do become disabled, and send a response with an error flag if not
return true;
}
//Damian Metody odpowiedzialne za awaryjne ztrzymanie silników
bool RosAriaNode::start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
stop_robot=true;
stop_motors_msg.data=false;
ROS_INFO("RosAria: Disable motors brakes.");
robot->lock();
if(robot->isEStopPressed())
ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
robot->unlock();
return true;
}
bool RosAriaNode::stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
robot->lock();
stop_robot=true;
stop_motors_msg.data=true;
//Zatrzymuje robota
robot->setVel(0);
if(robot->hasLatVel())
robot->setLatVel(0);
robot->setRotVel(0);
robot->comInt(ArCommands::ESTOP,0);
ROS_INFO("RosAria: Enable motors brakes.");
robot->unlock();
return true;
}
bool RosAriaNode::gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
robot->lock();
@ -892,7 +787,8 @@ void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
veltime = ros::Time::now();
watchdog= ros::Time::now();
if(stop_motors_msg.data)
if(!robot_state)
{
ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
return;