Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2010. David Feil-Seifer
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
- #include <ros/ros.h>
- #include <sensor_msgs/JointState.h>
- #include <sensor_msgs/Range.h>
- // For moving the robot on the map
- #include <tf/transform_broadcaster.h>
- #include <nav_msgs/Odometry.h>
- int main( int argc, char* argv[] )
- {
- ros::init(argc, argv, "p2os_publisher" );
- ros::NodeHandle n;
- ros::NodeHandle n_("~");
- ros::Rate loop_rate(10);
- ros::Publisher joint_state_publisher = n.advertise<sensor_msgs::JointState>("joint_states",1000);
- // Odometry test
- bool publish_odom = true;
- n_.param("publish_odom", publish_odom, publish_odom);
- ros::Publisher odom_pub;
- if(publish_odom) odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
- tf::TransformBroadcaster odom_broadcaster;
- // Sonar Range Test
- bool publish_sonar = true;
- n_.param("publish_sonar", publish_sonar, publish_sonar);
- ros::Publisher sonar_pub;
- if(publish_sonar) sonar_pub = n.advertise<sensor_msgs::Range>("Sonar", 1000);;
- sensor_msgs::Range sonar;
- sonar.radiation_type = sensor_msgs::Range::ULTRASOUND;
- sonar.field_of_view = (10.0/180.0) * 3.14;
- sonar.min_range = 0.0;
- sonar.max_range = 10.0;
- double wheel_rot = 0.0;
- double x = 0.0;
- double y = 0.0;
- double th = 0.0;
- double vx = 0.2;
- double vy = 0.0;
- double vth = vx; // Circle
- double vw = 1.0;
- ros::Time current_time, last_time;
- current_time = ros::Time::now();
- last_time = ros::Time::now();
- while( n.ok() )
- {
- current_time = ros::Time::now();
- double dt = (current_time - last_time).toSec();
- // Sonar Range Test
- if(publish_sonar)
- {
- for(int i=1;i<=16;i++)
- {
- sonar.header.stamp = ros::Time::now();
- sonar.range = 1.3;
- char buff[64];
- sprintf(buff, "Sonar_f%d", i);
- sonar.header.frame_id = buff;
- sonar_pub.publish(sonar);
- ros::spinOnce();
- loop_rate.sleep();
- }
- }
- // Wheel joints
- sensor_msgs::JointState js;
- wheel_rot += vw * dt;
- // P3-DX
- js.name.push_back(std::string("base_swivel_joint"));
- js.position.push_back(0);
- js.name.push_back(std::string("swivel_hubcap_joint"));
- js.position.push_back(wheel_rot);
- js.name.push_back(std::string("base_left_hubcap_joint"));
- js.position.push_back(wheel_rot);
- js.name.push_back(std::string("base_right_hubcap_joint"));
- js.position.push_back(wheel_rot);
- // P3-AT
- js.name.push_back(std::string("base_front_left_hub_joint"));
- js.position.push_back(wheel_rot);
- js.name.push_back(std::string("base_front_right_hub_joint"));
- js.position.push_back(wheel_rot);
- js.name.push_back(std::string("p3at_back_left_hub_joint"));
- js.position.push_back(wheel_rot);
- js.name.push_back(std::string("p3at_back_right_hub_joint"));
- js.position.push_back(wheel_rot);
- js.header.frame_id="base_link";
- js.header.stamp = ros::Time::now();
- joint_state_publisher.publish(js);
- // Odometry given the velocities of the robot
- if(publish_odom)
- {
- double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
- double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
- double delta_th = vth * dt;
- x += delta_x;
- y += delta_y;
- th += delta_th;
- // since all odometry is 6DOF we'll need a quaternion created from yaw
- geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
- // first, we'll publish the transform over tf
- geometry_msgs::TransformStamped odom_trans;
- odom_trans.header.stamp = current_time;
- odom_trans.header.frame_id = "odom";
- odom_trans.child_frame_id = "base_link";
- odom_trans.transform.translation.x = x;
- odom_trans.transform.translation.y = y;
- odom_trans.transform.translation.z = 0.0;
- odom_trans.transform.rotation = odom_quat;
- // send the transform
- odom_broadcaster.sendTransform(odom_trans);
- // next, we'll publish the odometry message over ROS
- nav_msgs::Odometry odom;
- odom.header.stamp = current_time;
- odom.header.frame_id = "odom";
- // set the position
- odom.pose.pose.position.x = x;
- odom.pose.pose.position.y = y;
- odom.pose.pose.position.z = 0.0;
- odom.pose.pose.orientation = odom_quat;
- // set the velocity
- odom.child_frame_id = "base_link";
- odom.twist.twist.linear.x = vx;
- odom.twist.twist.linear.y = vy;
- odom.twist.twist.angular.z = vth;
- // publish the message
- odom_pub.publish(odom);
- }
- ros::spinOnce();
- loop_rate.sleep();
- last_time = current_time;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement