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

Added code for obstacle detection

Browse Source
This commit is contained in:
Olek Bojda 2018-10-01 11:52:40 +02:00
parent 0baeeca94b
commit 1611a8d175
1 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
RosAria.cpp
View File

@ -605,30 +605,33 @@ void RosAriaNode::publishRobotInfo()
robot_info_msg.twist.angular.z = robot->getRotVel()*M_PI/180;
robot_info_msg.state.data = robot_state;
robot_info_msg.clutch.data = robot->areMotorsEnabled();
// robot_info_msg.header.stamp = ros::Time::now();
robot_info_msg.obstacle_detected.data = is_near_obstacle
robot_info_pub.publish(robot_info_msg);
}
void RosAriaNode::publish()
{
// Safety check
// is_near_obstacle = isCloseToObstacle();
// Safety distance check
is_near_obstacle = isCloseToObstacle();
// if (is_near_obstacle)
// {
// robot->lock();
if (is_near_obstacle)
{
robot->lock();
// robot->setVel(0);
// if(robot->hasLatVel())
// robot->setLatVel(0);
// robot->setRotVel(0);
robot->setVel(0);
if(robot->hasLatVel())
robot->setLatVel(0);
robot->setRotVel(0);
// robot->comInt(ArCommands::ESTOP,0); // ?
// robot->unlock();
robot->comInt(ArCommands::ESTOP,0); // ?
robot->unlock();
robot_state = false;
}
publishRobotInfo();
// robot_state = false;
// }
robot->requestEncoderPackets();
Wheel.header.stamp = ros::Time::now();
@ -803,9 +806,6 @@ void RosAriaNode::publish()
robot->setRotVel(0);
// ROS_INFO("WATCHDOG %d", WATCHDOG);
}
publishRobotInfo();
}
bool RosAriaNode::gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)