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