int main(int argc, char** argv)
{
ROS_INFO("Running babslam_main...");
srand (static_cast <unsigned> (time(0)));
ros::init(argc, argv, "babs_slam");
ros::NodeHandle nh_;
babs_slam babs(&nh_);
//sensor_model_test(babs);
//measurement_model_test(babs);
//motion_model_test(babs);
ros::Rate naptime(1/babs.dt);
bool warmedup = false;
while(!warmedup){
ROS_INFO("waiting for data");
warmedup=babs.warmCallbacks();
ros::spinOnce();
naptime.sleep();
}
while(ros::ok()){
babs.update();
ros::spinOnce();
naptime.sleep();
}
ros::spin();
return 0;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "minimal_publisher2"); // name of this node will be "minimal_publisher2"
ros::NodeHandle n; // two lines to create a publisher object that can talk to ROS
ros::Publisher my_publisher_object = n.advertise<std_msgs::Float64>("topic1", 1);
//"topic1" is the name of the topic to which we will publish
// the "1" argument says to use a buffer size of 1; could make larger, if expect network backups
std_msgs::Float64 input_float; //create a variable of type "Float64",
// as defined in: /opt/ros/indigo/share/std_msgs
// any message published on a ROS topic must have a pre-defined format,
// so subscribers know how to interpret the serialized data transmission
ros::Rate naptime(1.0); //create a ros object from the ros “Rate” class;
//set the sleep timer for 1Hz repetition rate (arg is in units of Hz)
input_float.data = 0.0;
// do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted
while (ros::ok())
{
// this loop has no sleep timer, and thus it will consume excessive CPU time
// expect one core to be 100% dedicated (wastefully) to this small task
input_float.data = input_float.data + 0.001; //increment by 0.001 each iteration
my_publisher_object.publish(input_float); // publish the value--of type Float64--
//to the topic "topic1"
//the next line will cause the loop to sleep for the balance of the desired period
// to achieve the specified loop frequency
naptime.sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "test_baxter_gripper");
ros::NodeHandle n;
ros::Publisher gripper_publisher_object = n.advertise<baxter_core_msgs::EndEffectorCommand>("/robot/end_effector/right_gripper/command", 1);
ros::Rate naptime(1.0);
//define and populate messages for gripper control:
baxter_core_msgs::EndEffectorCommand gripper_cmd_open, gripper_cmd_close;
gripper_cmd_open.command ="go";
gripper_cmd_open.args = "{'position': 100.0}'";
gripper_cmd_open.sender = "gripper_publisher";
gripper_cmd_close.command ="go";
gripper_cmd_close.args = "{'position': 0.0}'";
gripper_cmd_close.sender = "gripper_publisher";
// do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted
while (ros::ok())
{
//close the right gripper fully
gripper_publisher_object.publish(gripper_cmd_close);
naptime.sleep();
gripper_publisher_object.publish(gripper_cmd_open);
naptime.sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "urdf_rviz_commander"); // minimal_commander node
ros::NodeHandle n;
ros::Publisher joint_publisher = n.advertise<sensor_msgs::JointState>("joint_states", 1); // publish to vel_cmd topic
ros::Rate naptime(10); // update @ 10hz
double pi = 3.14159; // value of pi
double t = 0; // current time in calculation
double dt = 0.01; // timestep for calculation
double sine1; // sine output for joint1
double sine2; // sine output for joint2
double amplitude1 = pi; // amplitude value for joint1
double frequency1 = 1; // frequency value for joint1
double amplitude2 = pi; // amplitude value for joint2
double frequency2 = 4; // frequency value for joint2
std::vector<double> position(2, 0);
std::vector<std::string> name;
std::vector<std::string>::iterator nameIt;
name.push_back("joint1");
name.push_back("joint2");
sensor_msgs::JointState output; // message wrapper for sine output
while (ros::ok()) {
sine1 = amplitude1 * sin(2*pi*frequency1*t); // Calculate sine1
sine2 = amplitude2 * sin(2*pi*frequency2*t); // Calculate sine2
output.header.stamp = ros::Time::now();
output.name = name;
position[0] = sine1; // Store sine value in proper message format
position[1] = sine2;
output.position = position;
joint_publisher.publish(output); // Publish value to vel_cmd topic
t += dt; // Increment t by timeset dt
naptime.sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "hand_open_close_test");
ros::NodeHandle n;
//std::cout<<"enter motor_id to test: ";
int motor_id=1; // hard coded for bare motor
//std::cin>>motor_id;
char cmd_topic_name[50];
sprintf(cmd_topic_name,"dynamixel_motor%d_cmd",motor_id);
ROS_INFO("using command topic: %s",cmd_topic_name);
char cmd_topic_toggle[50];
sprintf(cmd_topic_toggle,"dynamixel_motor%d_mode",motor_id);
ros::Publisher dyn_pub = n.advertise<std_msgs::Int16>(cmd_topic_name, 1);
ros::Publisher torque_toggle = n.advertise<std_msgs::Bool>(cmd_topic_toggle, 1);
ros::Subscriber manual_cmd_subscriber = n.subscribe("gripper_cmd",1,manualCmdCB);
std_msgs::Int16 cmd_msg;
std_msgs::Bool toggle_msg;
double dt = 0.02; // repeat at freq 1/dt
ros::Rate naptime(1/dt); //create a ros object from the ros “Rate” class;
// initialize motor to position mode and open hand
toggle_msg.data = 0;
cmd_msg.data = 3000;
// do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted
while (ros::ok())
{
toggle_msg.data = input_command;
if (toggle_msg.data == 0)
{
ROS_INFO("sending open command");
cmd_msg.data = 3000; //setting open value (position)
}
if (toggle_msg.data == 1)
{
ROS_INFO("sending close command");
cmd_msg.data = 80; //setting close value (torque). This should firmly grip the larger blocks and loosely grip the smaller blocks
}
torque_toggle.publish(toggle_msg); // publish the operation mode (position/torque) to the motor node
dyn_pub.publish(cmd_msg); // publish the command to the motor node
ros::spinOnce();
naptime.sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "dynamixel_sin_test");
ros::NodeHandle n;
//std::cout<<"enter motor_id to test: ";
int motor_id=2; // hard coded for Yale hand
//std::cin>>motor_id;
char cmd_topic_name[50];
sprintf(cmd_topic_name,"dynamixel_motor%d_torque_cmd",motor_id);
ROS_INFO("using command topic: %s",cmd_topic_name);
ros::Publisher torque_pub = n.advertise<std_msgs::Int16>(cmd_topic_name, 1);
std_msgs::Int16 int_torque;
double dt = 0.02; // repeat at freq 1/dt
ros::Rate naptime(1/dt); //create a ros object from the ros “Rate” class;
int_torque.data = 0.0;
//double mid_angle = 3500;
//double amp = 500;
//double omega = 0.1*2*M_PI; // in Hz
//double phase = 0;
//double dbl_ang=0.0;
short int int_ang=3500; // mak237, initialize hand at mid position for safety
short int int_trq=0; // initialize torque to 0 to keep still
int cycle_counter = 0;
// do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted
while (ros::ok())
{
//phase += omega*dt;
//dbl_ang = amp*sin(phase)+mid_angle;
//int_ang = (short int) dbl_ang;
// mak237, modified code to move between two positions at set intervals
if (cycle_counter <= (2 * (1/dt))) // 2 is a magic number indicating holding the position for 2 seconds
{
int_trq = 0;
}
else if (cycle_counter > (2 * (1/dt))) // 2 is a magic number indicating changing the set torque
{
int_trq = 64;
}
int_torque.data = int_trq;
ROS_INFO("sending: %d",int_trq);
torque_pub.publish(int_torque); // publish the value--of type Float64--
naptime.sleep();
cycle_counter++; // increment counter to indicate a cycle has passed
// If more than 4 seconds have elapsed reset the counter to the initial state
if (cycle_counter >= (4 * (1/dt)))
{
cycle_counter = 0;
}
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "dynamixel_gripper_subscriber");
ros::NodeHandle n;
//std::cout<<"enter motor_id to test: ";
int motor_id=1; // hard coded for Yale hand
//std::cin>>motor_id;
char cmd_topic_name[50];
sprintf(cmd_topic_name,"dynamixel_motor%d_cmd",motor_id);
ROS_INFO("using command topic: %s",cmd_topic_name);
ros::Publisher dyn_pub = n.advertise<std_msgs::Int16>(cmd_topic_name, 1);
ros::Subscriber manual_cmd_subscriber = n.subscribe("manual_cmd",1,manualCmdCB);
std_msgs::Int16 int_angle;
double dt = 0.02; // repeat at freq 1/dt
ros::Rate naptime(1/dt); //create a ros object from the ros “Rate” class;
int_angle.data = 3000; // initialize hand at full open
int cycle_counter = 0;
// do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted
while (ros::ok())
{
// Using the input position as a boolean input
// 1 is open and 0 is close because any input other than 0 and 1 defaults to 1
// This should cause the hand to open in the event of a malformed command.
// If input is 1 open hand
if (gripper_pos == 1)
{
int_angle.data = 3000;
ROS_INFO("sending open command");
}
// If input is 2 close hand
else if (gripper_pos == 0)
{
int_angle.data = 3800;
ROS_INFO("sending close command");
}
dyn_pub.publish(int_angle);
ros::spinOnce();
naptime.sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "minimal_commander"); //initializing this node with the correct name for ROS
ros::NodeHandle handle; //designating node handle
//setup the service client for receiving data
ros::ServiceClient client = handle.serviceClient<msd78_assgn2::sine_params>("velocity_command_service");
msd78_assgn2::sine_params srv; //create service request message
srv.request.name = "velocity"; //set message type for correct service retrieval
//create a publisher for the vel_cmd subject to output the step values of the sinusoid
ros::Publisher sine_publisher = handle.advertise<std_msgs::Float64>("vel_cmd", 1);
std_msgs::Float64 vel; //published message for vel_cmd topic
int amp; //sinusoid amplitude, set by service
double freq; //sinusoid frequency, set by service
double time = 0; //stores time passed while running for the sine function
ros::Rate naptime(10.0); //set sleep rate at frequency rate - will only publish with new values
//while roscore is running, publish
while(ros::ok()) {
if(client.call(srv)) {
//need to store results from service request in local variables
amp = srv.response.amplitude;
freq = srv.response.frequency;
//calculate the sine as =amp*sine of (2*PI*freq*time)
vel.data = amp*sin(2*PI*freq*time);
//update time via the inverse of frequency
//the time passed for each cycle is 1/freq
time += 0.5;
//publish velocity
sine_publisher.publish(vel);
naptime.sleep(); //rest between cycle outputs
} else {
//failed to get a response from the service
ROS_INFO("Failed to receive a response from the client");
return 1;
}
}
return 0; //roscore has died
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "minimal_commander");
ROS_INFO("New node based on minimal_publisher node.");
double dt=.010;
ros::NodeHandle n;// two lines to create a publisher object that can talk to ROS
ros::Publisher my_publisher_object = n.advertise<std_msgs::Float64>("vel_cmd", 1);
// "vel_cmd" is the name of the topic to which we will publish
// the "1" argument says to use a buffer size of 1; could make larger, if expect network backups
ros::ServiceServer service = n.advertiseService("amp_freq", callback);
ROS_INFO("Ready to accept amplitude and frequency values.");
std_msgs::Float64 input_float, vel_cmd; //create a variable of type "Float64",
// as defined in: /opt/ros/indigo/share/std_msgs
// any message published on a ROS topic must have a pre-defined format,
// so subscribers know how to interpret the serialized data transmission
ros::Rate naptime(1/dt); //create a ros object from the ros “Rate” class;
//set the sleep timer for 100Hz repetition rate (arg is in units of Hz)
input_float.data = 0.0;
vel_cmd.data = 0.0;
// Based on the minimal_publisher node provided.
// do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted
while (ros::ok())
{
// this loop has no sleep timer, and thus it will consume excessive CPU time
// expect one core to be 100% dedicated (wastefully) to this small task
input_float.data = input_float.data + dt; // increment by 0.0001 each iteration
vel_cmd.data=amplitude*sin(2*PI*frequency*input_float.data);
my_publisher_object.publish(vel_cmd); // publish the value--of type Float64--
// to the topic "topic1"
// the next line will cause the loop to sleep for the balance of the desired period
// to achieve the specified loop frequency
naptime.sleep();
ros::spinOnce();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "minimal_controller"); //name this node
// when this compiled code is run, ROS will recognize it as a node called "minimal_controller"
ros::NodeHandle nh; // node handle
ros::Subscriber my_subscriber_object2 = nh.subscribe("pos_cmd", 1, myCallbackPosCmd);
ros::Publisher my_publisher_object = nh.advertise<std_msgs::Float64>("vel_cmd", 1);
double a = 1.0; // velocity feedback gain
double dt_controller = 0.1; //specify 10Hz controller sample rate (pretty slow, but
//illustrative)
double sample_rate = 1.0 / dt_controller; // compute the corresponding update frequency
ros::Rate naptime(sample_rate); // use to regulate loop rate
g_vel.data = 0.0; // initialize force to 0; will get updated by callback
g_pos_cmd.data = 0.0; // init velocity command to zero
double vel_max = 0.0;
double d = 0.0;
int p = 0;
while (ros::ok()) {
d = g_pos_cmd.data - 0;
vel_max = sqrt(d*a);
if(d>0){
if(p == 0 && g_vel.data <= vel_max){g_vel.data = g_vel.data + a*dt_controller;}
else if (p == 0 && g_vel.data >= vel_max){p = 1;}
else if (p == 1 && g_vel.data >= 0){g_vel.data = g_vel.data - a*dt_controller;}
else if (p == 1 && g_vel.data <= 0){p = 2;}
else if (p == 2){g_vel.data = 0;}
}
my_publisher_object.publish(g_vel); // publish the control effort computed by this
//controller
ROS_INFO("velocity command = %f, %i, %f", g_vel.data, p, vel_max);
ros::spinOnce(); //allow data update from callback;
naptime.sleep(); // wait for remainder of specified period;
}
return 0; // should never get here, unless roscore dies
}
int main(int argc, char **argv) {
ros::init(argc, argv, "example_ros_message_publisher"); // name of this node
ros::NodeHandle n; // two lines to create a publisher object that can talk to ROS
ros::Publisher my_publisher_object = n.advertise<example_ros_msg::example_message>("example_topic", 1);
//"example_topic" is the name of the topic to which we will publish
// the "1" argument says to use a buffer size of 1; could make larger, if expect network backups
example_ros_msg::example_message my_new_message;
//create a variable of type "example_msg",
// as defined in this package
ros::Rate naptime(1.0); //create a ros object from the ros “Rate” class;
//set the sleep timer for 1Hz repetition rate (arg is in units of Hz)
// put some data in the header. Do: rosmsg show std_msgs/Header
// to see the definition of "Header" in std_msgs
my_new_message.header.stamp = ros::Time::now(); //set the time stamp in the header;
my_new_message.header.seq=0; // call this sequence number zero
my_new_message.header.frame_id = "base_frame"; // would want to put true reference frame name here, if needed for coord transforms
my_new_message.demo_int= 1;
my_new_message.demo_double=100.0;
double sqrt_arg;
// do work here in infinite loop (desired for this example), but terminate if detect ROS has faulted
while (ros::ok())
{
my_new_message.header.seq++; //increment the sequence counter
my_new_message.header.stamp = ros::Time::now(); //update the time stamp
my_new_message.demo_int*=2.0; //double the integer in this field
sqrt_arg = my_new_message.demo_double;
my_new_message.demo_double = sqrt(sqrt_arg);
my_publisher_object.publish(my_new_message); // publish the data in new message format on topic "example_topic"
//the next line will cause the loop to sleep for the balance of the desired period
// to achieve the specified loop frequency
naptime.sleep();
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "minimal_commander"); // minimal_commander node
ros::NodeHandle n;
ros::Publisher command_publisher = n.advertise<std_msgs::Float64>("vel_cmd", 1); // publish to vel_cmd topic
ros::Rate naptime(10); // update @ 10hz
double pi = 3.14159; // value of pi
double t = 0; // current time in calculation
double dt = 0.1; // timestep for calculation
double sine; // sine output
double amplitude = 1; // amplitude value for sine
double frequency = 1; // frequency value for sine
std_msgs::Float64 output; // message wrapper for sine output
while (ros::ok()) {
sine = amplitude * sin(2*pi*frequency*t); // Calculate sine value
output.data = sine; // Store sine value in proper message format
command_publisher.publish(output); // Publish value to vel_cmd topic
t += dt; // Increment t by timeset dt
naptime.sleep();
}
}
