smart_motor_node

#include <csignal>
#include <cstdio>
#include "ros/ros.h"
#include "toro/motorInterface.h"
#include <math.h>
#include <toro/Sweep.h>

CMotorInterface motorInterface;

void sweep_callback(const toro::Sweep::ConstPtr&/*toro::State::ConstPtr&*/ sweep_command)
{
	// Start the motor sweeping
	motorInterface.setStart(sweep_command->start);
	motorInterface.setEnd(sweep_command->end);
	motorInterface.setSpeed(sweep_command->speed);
	motorInterface.startSweeping();

	double startTime = ros::Time::now().toSec();
	double endTime = startTime+sweep_command->duration;

	ros::Rate loop_rate(10); //set loop rate to 10 hz

	while (ros::ok() && ros::Time::now().toSec()<endTime)
	{
		//wait to eventually end sweep
		loop_rate.sleep();
	}
	//now that time has run out, or user has killed the program, stop sweeping
	motorInterface.stopSweeping();

}

int main(int argc, char **argv)
{

	//smart motor interface stuff
	const char *port = "/dev/ttyUSB0";

	ros::init(argc, argv, "smart_motor_node");
	ros::NodeHandle n;

	//connect to smart motor
	ROS_INFO("opening serial port %s", port);
	try
	{
		motorInterface.open(port);
	} catch (ESMSerial &exception)
	{
		ROS_INFO("Failed to open smart motor serial port");
		return(0);
	}
	//home smart motor
	int position, oldPosition, positionSame;
	try
	{
		ROS_INFO("Homing smart motor");
		if (!motorInterface.homeMotor())
		{
			ROS_INFO("Failed to home smart motor");
			return(0);
		}
		// Wait for position to settle
		positionSame = oldPosition = 0;
		do
		{
			position = motorInterface.getPosition();
			ROS_INFO("position: %d, oldPosition: %d", position, oldPosition);
			if (oldPosition == position)
			{
				positionSame++;
			} else
			{
				positionSame = 0;
			}
			oldPosition = position;
		} while (positionSame < 10);
	} catch (ESMSerial &exception)
	{
		ROS_INFO("Failed to home smart motor");
		return(0);
	}
	ROS_INFO("Final position: %d", position);

	//set up a custom message subscriber
	ros::Subscriber sweep_subscriber = n.subscribe<toro::Sweep>("/sweep", 1, sweep_callback);

	ros::spin();
}