int main(int argc, char **argv) {
ros::init(argc, argv, "cmd_vel_switch");
ros::NodeHandle nh;
ros::Publisher cmder = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
ros::Subscriber modesub= nh.subscribe("cmd_mode",1,modecb);
ros::Subscriber joysub= nh.subscribe("joystick/cmd_vel",1,joycmd);
ros::Subscriber rvizsub= nh.subscribe("rviz/cmd_vel",1,rvizcmd);
ros::Subscriber gpssub= nh.subscribe("gps_bug/cmd_vel",1,gpscmd);
mode.data = 0;
ros::Rate looprate(LOOP_RATE);
while (ros::ok())
{
if (mode.data==0){
cmder.publish(joy);
}
if (mode.data==1){
cmder.publish(rviz);
}
if (mode.data==2){
cmder.publish(bug);
}
looprate.sleep();
ros::spinOnce();
}
}
int main(int argc,char** argv)
{
ros::init(argc ,argv, "seabotix");
ros::NodeHandle n;
ros::Subscriber _sub4 = n.subscribe<kraken_msgs::thrusterData4Thruster>(topics::CONTROL_PID_THRUSTER4,2,thruster4callback);
ros::Subscriber _sub6 = n.subscribe<kraken_msgs::thrusterData6Thruster>(topics::CONTROL_PID_THRUSTER6,2,thruster6callback);
ros::Publisher _pub = n.advertise<kraken_msgs::seabotix>(topics::CONTROL_SEABOTIX,2);
serial arduino;
a
ros::Rate looprate(10);
for(int i=0; i<12; i++)
_output.data[i] = i;
while(ros::ok())
{
_pub.publish(_output);
sleep(2);
ros::spinOnce();
looprate.sleep();
}
return 0;
}
void MicView::spin(){
ros::Rate looprate(50);
while(ros::ok()){
ros::spinOnce();
sendMicData();
looprate.sleep();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ardu_core");
ros::NodeHandle n;
head_turn_pub = n.advertise<std_msgs::UInt16>("head_turn", 1000);
movement_pub = n.advertise<std_msgs::UInt16>("movement", 1000);
ros::Rate looprate(10);
while(ros::ok())
{
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "des_state_publisher");
ros::NodeHandle nh;
//instantiate a desired-state publisher object
DesStatePublisher desStatePublisher(nh);
//dt is set in header file pub_des_state.h
ros::Rate looprate(1 / dt); //timer for fixed publication rate
desStatePublisher.set_init_pose(0,0,0); //x=0, y=0, psi=0
//put some points in the path queue--hard coded here
// main loop; publish a desired state every iteration
while (ros::ok()) {
desStatePublisher.pub_next_state();
ros::spinOnce();
looprate.sleep(); //sleep for defined sample period, then do loop again
}
}
void analysisCB(const task_commons::buoyGoalConstPtr goal)
{
ROS_INFO("Inside analysisCB");
heightCenter = false;
sideCenter = false;
successBuoy = false;
IP_stopped = false;
heightGoal = false;
ros::Rate looprate(12);
if (!buoy_server_.isActive())
return;
ROS_INFO("Waiting for Forward server to start.");
ForwardClient_.waitForServer();
SidewardClient_.waitForServer();
UpwardClient_.waitForServer();
TurnClient_.waitForServer();
TaskBuoyInnerClass::startBuoyDetection();
sidewardgoal.Goal = 0;
sidewardgoal.loop = 10;
SidewardClient_.sendGoal(sidewardgoal);
boost::thread spin_thread_sideward_camera(&TaskBuoyInnerClass::spinThreadSidewardCamera, this);
// Stabilization of yaw
turngoal.AngleToTurn = 0;
turngoal.loop = 100000;
TurnClient_.sendGoal(turngoal);
upwardgoal.Goal = 0;
upwardgoal.loop = 10;
UpwardClient_.sendGoal(upwardgoal);
boost::thread spin_thread_upward_camera(&TaskBuoyInnerClass::spinThreadUpwardCamera, this);
while (goal->order)
{
if (buoy_server_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
buoy_server_.setPreempted();
successBuoy = false;
break;
}
looprate.sleep();
if (heightCenter && sideCenter)
{
break;
}
// publish the feedback
feedback_.nosignificance = false;
buoy_server_.publishFeedback(feedback_);
ROS_INFO("x = %f, y = %f, front distance = %f", data_X_.data, data_Y_.data, data_distance_.data);
ros::spinOnce();
}
ROS_INFO("Bot is in center of buoy");
forwardgoal.Goal = 0;
forwardgoal.loop = 10;
ForwardClient_.sendGoal(forwardgoal);
while (goal->order)
{
if (buoy_server_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
buoy_server_.setPreempted();
successBuoy = false;
break;
}
looprate.sleep();
if (IP_stopped)
{
successBuoy = true;
ROS_INFO("Waiting for hitting the buoy...");
sleep(1);
break;
}
feedback_.nosignificance = false;
buoy_server_.publishFeedback(feedback_);
ROS_INFO("x = %f, y = %f, front distance = %f", data_X_.data, data_Y_.data, data_distance_.data);
ros::spinOnce();
}
ForwardClient_.cancelGoal(); // stop motion here
upwardgoal.Goal = present_depth + 5;
upwardgoal.loop = 10;
UpwardClient_.sendGoal(upwardgoal);
ROS_INFO("moving upward");
boost::thread spin_thread_upward_pressure(&TaskBuoyInnerClass::spinThreadUpwardPressure, this);
while (!heightGoal)
{
ROS_INFO("present depth = %f", present_depth);
looprate.sleep();
ros::spinOnce();
}
result_.MotionCompleted = successBuoy && heightGoal;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
buoy_server_.setSucceeded(result_);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "human_node_grid");
ros::NodeHandle n;
int loop_rate;
ros::param::param("~/loop_rate", loop_rate, 5);
ros::Rate looprate(loop_rate);
// bool get_leg_grid = true;
// bool get_sound_grid = true;
// bool get_torso_grid = true;
double lw, sw, tw;
// ros::param::param("/human_grid_node/leg_node", get_leg_grid, true);
// ros::param::param("/human_grid_node/torso_node", get_torso_grid, true);
// ros::param::param("/human_grid_node/sound_node", get_sound_grid, true);
ros::param::param("/human_grid_node/sound_weight", sw, 4.0);
ros::param::get("/human_grid_node/sound_weight", sw);
ros::param::param("/human_grid_node/torso_weight",tw, 3.0);
ros::param::get("/human_grid_node/torso_weight", tw);
ros::param::param("/human_grid_node/leg_weight", lw, 2.0);
ros::param::get("/human_grid_node/leg_weight", lw);
int probability_projection_step;
ros::param::param("~/human/probability_projection_step",probability_projection_step , 1);
CHumanGrid human_grid(n,lw, sw, tw, probability_projection_step);
ros::Subscriber leg_grid_sub = n.subscribe("leg/probability", 10,
&CHumanGrid::legCallBack,
&human_grid);
ros::Subscriber sound_grid_sub = n.subscribe("sound/probability", 10,
&CHumanGrid::soundCallBack,
&human_grid);
ros::Subscriber torso_grid_sub = n.subscribe("torso/probability", 10,
&CHumanGrid::torsoCallBack,
&human_grid);
ros::Subscriber encoder_sub = n.subscribe("husky/odom", 10,
&CHumanGrid::encoderCallBack, &human_grid);
ros::Subscriber weights_sub = n.subscribe("/weights", 10,
&CHumanGrid::weightsCallBack, &human_grid);
while(ros::ok())
{
human_grid.integrateProbabilities();
if(looprate.cycleTime() > looprate.expectedCycleTime())
ROS_ERROR("It is taking too long! %f", looprate.cycleTime().toSec());
if(!looprate.sleep())
ROS_ERROR("Not enough time left");
ros::spinOnce();
}
return 0;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "des_state_publisher");
ros::NodeHandle nh;
ros::Subscriber odom_subscriber = nh.subscribe("odom", 1, odomCallback);
ros::Publisher start_publisher = nh.advertise<std_msgs::Bool>("start_trigger", 1, true); // talks to the robot!
std_msgs::Bool start_cmd;
RobotCommander robot(&nh);
//instantiate a desired-state publisher object
DesStatePublisher desStatePublisher(nh);
//dt is set in header file pub_des_state.h
ros::Rate looprate(1 / dt); //timer for fixed publication rate
ROS_WARN("You can now move the robot, input anything to start");
getchar();
ROS_INFO("Waiting for good amcl particles");
robot.turn(M_PI*2);
//ROS_INFO("Waiting for odometery message to start...");
while (!is_init_orien) {
ros::spinOnce(); //wait for odom callback
}
ROS_INFO("got odometery with x = %f, y = %f, th = %f", g_current_pose.position.x, g_current_pose.position.y, quat2ang(g_current_pose.orientation));
desStatePublisher.set_init_pose(g_current_pose.position.x, g_current_pose.position.y, quat2ang(g_current_pose.orientation)); //x=0, y=0, psi=0
double gcpX = g_current_pose.position.x;
double gcpY = g_current_pose.position.y;
double gcpTh = quat2ang(g_current_pose.orientation);
start_cmd.data = true;
start_publisher.publish(start_cmd);
if (argc > 1 && ( strcmp(argv[1], "jinx") == 0 )) {
double x = 5.6 + gcpX;
double y = -12.4 + gcpY;
double v = 6.1 + 1.8 - 0.3; // 20 tiles + 3yd - 1ft for safety
desStatePublisher.append_path_queue(x, gcpY, gcpTh );
desStatePublisher.append_path_queue(x, gcpY, gcpTh-M_PI/2);
desStatePublisher.append_path_queue(x, y, gcpTh-M_PI/2);
desStatePublisher.append_path_queue(x-v, y, gcpTh-M_PI );
} else if (argc > 1 && ( strcmp(argv[1], "test") == 0 )) {
desStatePublisher.append_path_queue(0.5, 0.0, 0.0);
} else if (argc > 1 && ( strcmp(argv[1], "circle_out") == 0 )) {
double circle_length = 2.0;
/* clockwise*/
desStatePublisher.append_path_queue(circle_length, 0.0, 0.0);
desStatePublisher.append_path_queue(circle_length, circle_length, -M_PI/2);
desStatePublisher.append_path_queue(-circle_length, circle_length, -M_PI);
desStatePublisher.append_path_queue(-circle_length, -circle_length, M_PI/2);
desStatePublisher.append_path_queue(circle_length, -circle_length, 0.0);
desStatePublisher.append_path_queue(circle_length, circle_length, -M_PI/2);
/*
desStatePublisher.append_path_queue(circle_length, 0.0, -M_PI/2);
desStatePublisher.append_path_queue(circle_length, -circle_length, -M_PI);
desStatePublisher.append_path_queue(-circle_length, -circle_length, M_PI/2);
desStatePublisher.append_path_queue(-circle_length, circle_length, 0.0);
*/
desStatePublisher.append_path_queue(0.0, circle_length, M_PI/2);
desStatePublisher.append_path_queue(0.0, 7.0, M_PI);
desStatePublisher.append_path_queue(-9.0, 7.0, -M_PI/2);
desStatePublisher.append_path_queue(-9.0, 0.0, -M_PI/2);
} else if (argc > 1 && ( strcmp(argv[1], "out") == 0 )) {
desStatePublisher.append_path_queue(0.0, 7.0, -M_PI/2);
desStatePublisher.append_path_queue(-9.0, 7.0, -M_PI);
desStatePublisher.append_path_queue(-9.0, 0.0, M_PI/2);
} else {
desStatePublisher.append_path_queue(0.0, 0.0, 0.0);
}
// main loop; publish a desired state every iteration
while (ros::ok()) {
desStatePublisher.pub_next_state();
ros::spinOnce();
looprate.sleep(); //sleep for defined sample period, then do loop again
}
}
void analysisCB(const task_commons::lineGoalConstPtr goal)
{
ROS_INFO("Inside analysisCB");
success = true;
isOrange = false;
LineAlign = false;
FrontCenter = false;
SideCenter = false;
ros::Rate looprate(12);
if (!line_server_.isActive())
return;
ROS_INFO("%s Waiting for Forward server to start.", action_name_.c_str());
ForwardClient_.waitForServer();
SidewardClient_.waitForServer();
TurnClient_.waitForServer();
TaskLineInnerClass::detection_switch_on();
// Stabilization of yaw
turngoal.AngleToTurn = 0;
turngoal.loop = 100000;
TurnClient_.sendGoal(turngoal);
// start moving forward.
// send initial data to forward.
forwardgoal.Goal = 100;
forwardgoal.loop = 10;
ForwardClient_.sendGoal(forwardgoal);
ROS_INFO("%s: searching line", action_name_.c_str());
while (goal->order && success)
{
if (line_server_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
line_server_.setPreempted();
success = false;
break;
}
looprate.sleep();
if (isOrange)
{
// stop will happen outside while loop so if preempted then too it will
// stop
break;
}
// publish the feedback
feedback_.AngleRemaining = angle_goal.data;
feedback_.x_coord = data_X_.data;
feedback_.y_coord = data_Y_.data;
line_server_.publishFeedback(feedback_);
ros::spinOnce();
}
ForwardClient_.cancelGoal(); // stop motion here
TaskLineInnerClass::detection_switch_off();
TaskLineInnerClass::centralize_switch_on();
sidewardgoal.Goal = 0;
sidewardgoal.loop = 10;
SidewardClient_.sendGoal(sidewardgoal);
boost::thread spin_thread_sideward_camera(&TaskLineInnerClass::spinThreadSidewardCamera, this);
forwardgoal.Goal = 0;
forwardgoal.loop = 10;
ForwardClient_.sendGoal(forwardgoal);
boost::thread spin_thread_forward_camera(&TaskLineInnerClass::spinThreadForwardCamera, this);
ROS_INFO("%s: line is going to be centralized", action_name_.c_str());
while (goal->order && success)
{
if (line_server_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
line_server_.setPreempted();
success = false;
break;
}
looprate.sleep();
if (FrontCenter && SideCenter)
{
ROS_INFO("%s: Line has been centralized.", action_name_.c_str());
break;
}
// publish the feedback
feedback_.AngleRemaining = angle_goal.data;
feedback_.x_coord = data_X_.data;
feedback_.y_coord = data_Y_.data;
line_server_.publishFeedback(feedback_);
ros::spinOnce();
}
TaskLineInnerClass::centralize_switch_off();
TaskLineInnerClass::angle_switch_on();
TurnClient_.cancelGoal(); // stopped stablisation
while (angle_goal.data == 0.0)
{
sleep(.01);
}
turngoal.AngleToTurn = angle_goal.data;
turngoal.loop = 10;
TurnClient_.sendGoal(turngoal);
boost::thread spin_thread_turn_camera(&TaskLineInnerClass::spinThreadTurnCamera, this);
ROS_INFO("%s: line is going to be aligned", action_name_.c_str());
while (goal->order && success)
{
if (line_server_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
line_server_.setPreempted();
success = false;
break;
}
looprate.sleep();
if (LineAlign)
{
ROS_INFO("%s: line is aligned.", action_name_.c_str());
break;
}
feedback_.AngleRemaining = angle_goal.data;
feedback_.x_coord = data_X_.data;
feedback_.y_coord = data_Y_.data;
line_server_.publishFeedback(feedback_);
ros::spinOnce();
}
TaskLineInnerClass::angle_switch_off();
result_.MotionCompleted = success;
ROS_INFO("%s: Success is %s", action_name_.c_str(), success ? "true" : "false");
line_server_.setSucceeded(result_);
}
int main(int argc, char **argv)
{
ros::init(argc,argv,"convert_twist");
ros::NodeHandle nh_;
ros::Subscriber cmd_converter= nh_.subscribe("cmd_vel",1,twistCallback);
ros::Subscriber fdbk1= nh_.subscribe("ch1/feedback",1,feedback1);
ros::Subscriber fdbk2= nh_.subscribe("ch2/feedback",1,feedback2);
ch1 = nh_.advertise<roboteq_msgs::Command>("ch1/cmd", 1);
ch2 = nh_.advertise<roboteq_msgs::Command>("ch2/cmd", 1);
ros::Publisher joints = nh_.advertise<sensor_msgs::JointState>("joint_states",1);
g_pos1 = 0;
g_pos2 = 0;
g_vel1 = 0;
g_vel2 = 0;
g_last_sent_cmd.linear.x = 0;
g_last_sent_cmd.angular.z = 0;
ros::Rate looprate(LOOP_RATE);
while (ros::ok()){
sensor_msgs::JointState state;
state.name.push_back("ch1");
state.position.push_back(g_pos1);
state.velocity.push_back(g_vel1);
state.name.push_back("ch2");
state.position.push_back(g_pos2);
state.velocity.push_back(g_vel2);
state.header.stamp = ros::Time::now();
joints.publish(state);
looprate.sleep();
ros::spinOnce();
geometry_msgs::Twist cmd_to_motor = g_last_received_cmd;
float dv_lin = cmd_to_motor.linear.x-g_last_sent_cmd.linear.x;
float dv_ang = cmd_to_motor.angular.z-g_last_sent_cmd.angular.z;
if(dv_lin>MAX_LIN_ACEL/LOOP_RATE){
cmd_to_motor.linear.x = g_last_sent_cmd.linear.x + MAX_LIN_ACEL/LOOP_RATE;
//ROS_INFO("limiting from %f to %f", g_last_received_cmd.linear.x, cmd_to_motor.linear.x);
}else if(dv_lin*-1.0>MAX_LIN_ACEL/LOOP_RATE){
cmd_to_motor.linear.x = g_last_sent_cmd.linear.x - MAX_LIN_ACEL/LOOP_RATE;
}
if(dv_ang > MAX_ANG_ACEL/LOOP_RATE){
cmd_to_motor.angular.z = g_last_sent_cmd.angular.z + MAX_ANG_ACEL/LOOP_RATE;
}else if(dv_ang*-1.0>MAX_ANG_ACEL/LOOP_RATE){
cmd_to_motor.angular.z = g_last_sent_cmd.angular.z - MAX_ANG_ACEL/LOOP_RATE;
}
g_last_sent_cmd = cmd_to_motor;
float v1,v2;
v1 = cmd_to_motor.linear.x;
v2 = cmd_to_motor.linear.x;
v1*=MAX_COMMAND/MAX_SPEED*DRIVING_DIRECTION;
v2*=MAX_COMMAND/MAX_SPEED*DRIVING_DIRECTION;
v2 += (cmd_to_motor.angular.z)*MAX_COMMAND/MAX_SPEED*BASE_RADIUS;
v1 -= (cmd_to_motor.angular.z)*MAX_COMMAND/MAX_SPEED*BASE_RADIUS;
v1/= LEFT_OVER_RIGHT;
v2*= LEFT_OVER_RIGHT;
v1 = std::max(-1.0f*MAX_COMMAND,std::min(v1,MAX_COMMAND));
v2 = std::max(-1.0f*MAX_COMMAND,std::min(v2,MAX_COMMAND));
roboteq_msgs::Command cmd1, cmd2;
cmd1.mode = 0;
cmd2.mode = 0;
cmd1.setpoint = v1;
cmd2.setpoint = v2;
ch1.publish(cmd1);
ch2.publish(cmd2);
}
return 0;
}
