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

Merge branch 'master' of https://bitbucket.org/Olek_Bojda/pioneer-software

Browse Source
This commit is contained in:
Pioneer3 2018-10-01 11:57:31 +02:00
commit b0b1c61705
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

@ -627,30 +627,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();
@ -825,9 +828,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)