//MAIN
int main(int argc, char** argv)
{
ros::init(argc, argv, "wheel_motor_node"); //Initialize node
ros::NodeHandle n; //Create nodehandle object
sub = n.subscribe("wheel_motor_rc", 1000, callBack); //Create object to subscribe to topic "wheel_motor_rc"
pub = n.advertise<motor_controller::AdaCmd>("adaFruit",1000); //Create object to publish to topic "I2C"
while(pub.getNumSubscribers()==0);//Prevents message from sending when publisher is not completely connected to subscriber.
//ros::Rate loop_rate(10); //Set frequency of looping. 10 Hz
setGPIOWrite(33,1); //Motor enable
while(ros::ok())
{
//Update time
current_time = ros::Time::now();
//Check if interval has passed
if(current_time - last_time > update_rate)
{
//Reset time
last_time = current_time;
if(sub.getNumPublishers() == 0) //In case of loss of connection to publisher, set controller outputs to 0
{
ROS_WARN("Loss of wheel motor controller input!");
leftFrontWheel.setU(0); //Set controller inputs
leftRearWheel.setU(0);
rightRearWheel.setU(0);
rightFrontWheel.setU(0);
}
controlFunction(); //Motor controller function
pub.publish(generateMessage());
}
ros::spinOnce();
}
stopAllMotors();
return 0;
}
MyLittleTurtle(ros::NodeHandle& nodeHandle): m_nodeHandle(nodeHandle)
{
ROS_INFO("Publishing topic...");
m_turtlesimPublisher = m_nodeHandle.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
ROS_INFO("Subscribing to turtlesim topic...");
m_turtlesimSubscriber = m_nodeHandle.subscribe("/turtle1/pose", 10, &MyLittleTurtle::updateTurtleStatus, this);
ROS_INFO("Waiting for turtlesim...");
ros::Rate loopRate(10);
while (ros::ok() && (m_turtlesimPublisher.getNumSubscribers() <= 0 || m_turtlesimSubscriber.getNumPublishers() <= 0))
loopRate.sleep();
checkRosOk_v();
ROS_INFO("Retrieving initial status...");
ros::spinOnce();
ROS_INFO("Initial status: x=%.3f, y=%.3f, z=%.3f", m_status.x, m_status.y, m_status.z);
ROS_INFO("Everything's ready.");
}
// move platform server receives a new goal
void moveGoalCB()
{
set_terminal_state_ = true;
move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);
if (as_.isPreemptRequested() ||!ros::ok())
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
if (planning_)
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
if (controlling_)
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.result_state = 0;
move_result_.error_string = "Preempt Requested!!!";
as_.setPreempted(move_result_);
return;
}
// Convert move goal to plan goal
pose_goal_.pose_goal = move_goal_.nav_goal;
if (planner_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
planning_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Planner is down";
as_.setAborted(move_result_);
}
else
{
ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
planning_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
}
return;
}
void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result)
{
planning_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString());
move_result_.result_state = result->result_state;
if (move_result_.result_state) //if plan OK
{
planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner
if (ctrl_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down");
controlling_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Controller is down!!!";
as_.setAborted(move_result_);
}
else
{
ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1));
controlling_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller");
}
}
else //if plan NOT OK
{
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.error_string = "Planning Failed: " + result->error_string;
ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string);
as_.setAborted(move_result_);
}
return;
}
bool IsNodeReady()
{
return (pub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0);
}
//publish state timer
void state_pub_timerCB(const ros::TimerEvent& event)
{
if (ctrl_state_sub.getNumPublishers()==0)
{
hw_state_.state = hardware::ERROR;
hw_state_.description = "Platform motion controller is down" ;
state_pub_.publish(hw_state_);
return;
}
if (planner_state_sub.getNumPublishers()==0)
{
hw_state_.state = hardware::ERROR;
hw_state_.description = "Platform motion planner is down" ;
state_pub_.publish(hw_state_);
return;
}
if (plan_state_ == hardware::BUSY)
{
hw_state_.state = hardware::BUSY;
hw_state_.description = "Robot is planning" ;
}
else
{
switch (control_state_)
{
case 0:
hw_state_.state = hardware::IDLE;
hw_state_.description = "Ready to receive a goal" ; // - What is zero?
break;
case 1:
hw_state_.state = hardware::BUSY;
hw_state_.description = "Robot is moving at normal speed" ;
break;
case 2:
hw_state_.state = hardware::BUSY;
hw_state_.description = "Robot is moving at slow speed" ;
break;
case 5:
hw_state_.state = hardware::IDLE;
hw_state_.description = "Ready to receive a goal" ;
break;
case 10:
hw_state_.state = hardware::ERROR;
hw_state_.description = "Robot is in EMERGENCY state" ;
break;
case 20:
hw_state_.state = hardware::BUSY;
hw_state_.description = "Some obstacle in the way of the robot" ;
break;
case 21:
hw_state_.state = hardware::ERROR;
hw_state_.description = "Immovable obstacle in the way of the robot" ;
break;
case 30:
hw_state_.state = hardware::ERROR;
hw_state_.description = "Localization NOT OK" ;
break;
}
}
state_pub_.publish(hw_state_);
return;
}