Commit 80512049 authored by Prasenjit Mukherjee's avatar Prasenjit Mukherjee
Browse files

moving husky_navigation to its own repo under clearpath/husky, deleting non-associated packages

parent 0e11aac5
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of
# directories (or patterns, but directories should suffice) that should
# be excluded from the distro. This is not the place to put things that
# should be ignored everywhere, like "build" directories; that happens in
# rosbuild/rosbuild.cmake. Here should be listed packages that aren't
# ready for inclusion in a distro.
#
# This list is combined with the list in rosbuild/rosbuild.cmake. Note
# that CMake 2.6 may be required to ensure that the two lists are combined
# properly. CMake 2.4 seems to have unpredictable scoping rules for such
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
rosbuild_make_distribution(0.3.4)
# This is a test of CI
include $(shell rospack find mk)/cmake_stack.mk
clearpath_husky
===============
Legacy rosbuild packages for Husky. Please use ROS Hydro; packages are on the new Husky org:
https://github.com/husky
As always, the most up-to-date information may be found on the ROS wiki:
http://wiki.ros.org/Robots/Husky
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
rosbuild_init()
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
# Add dynamic reconfigure api
rosbuild_find_ros_package(dynamic_reconfigure)
include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
gencfg()
include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
apps:
- display: Android Teleop
app: husky_android_teleop/android_teleop
#!/usr/bin/python
import sys
print "clearpath/%s" % sys.argv[1].split('/', 2)[1]
#! /usr/bin/env python
PKG = "husky_bringup"
import roslib; roslib.load_manifest(PKG)
from dynamic_reconfigure.parameter_generator import *
gen = ParameterGenerator()
gen.add("gyro_scale_correction", double_t, 0, "Scaling factor for correct gyro operation.", 1.0, 0.0, 3.0)
exit( gen.generate(PKG, "clearpath_husky", "Husky"))
<launch>
<!-- App manager for Android devices -->
<param name="robot/name" value="husky"/>
<param name="robot/type" value="husky"/>
<node pkg="app_manager" type="appmaster" name="appmaster" args="-p 11312"/>
<node pkg="app_manager" type="app_manager" name="app_manager" args="--applist $(find husky_bringup)/apps" output="screen">
<param name="interface_master" value="http://localhost:11312"/>
</node>
</launch>
<launch>
<arg name="port" default="/dev/prolific" />
<node pkg="clearpath_base" type="kinematic.py" name="clearpath_base" ns="husky">
<param name="port" value="$(arg port)" />
<param name="cmd_fill" value="True" />
<param name="data/system_status" value="10" />
<param name="data/safety_status" value="10" />
<param name="data/encoders" value="10" />
<param name="data/differential_speed" value="10" />
<param name="data/differential_output" value="10" />
<param name="data/power_status" value="1" />
</node>
</launch>
<launch>
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find husky_description)/urdf/base.urdf.xacro'" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</launch>
<launch>
<!-- Provide interactive markers capability -->
<node pkg="husky_interactive_markers" name="interactive_markers" type="husky_marker_server">
<remap from="cmd_vel" to="husky/cmd_vel"/>
</node>
<!-- Publish diagnostics information -->
<node pkg="husky_bringup" name="diagnostics" type="diagnostics.py"/>
<!-- Subscribe to encoders and optionally IMU -->
<node pkg="husky_bringup" name="dead_reckoning" type="dead_reckoning.py">
<param name="gyro_scale_correction" value="1.0" />
</node>
</launch>
<launch>
<!-- Teleop -->
<include file="$(find clearpath_teleop)/teleop.launch">
<arg name="drive_speed" value="1.0" />
<arg name="turn_speed" value="1.0" />
<arg name="cmd_topic" value="husky/cmd_vel" />
</include>
</launch>
<launch>
<node pkg="imu_um6" type="imu_um6_node.py" name="imu_um6_node" respawn="true">
<param name="port" type="string" value="/dev/clearpath/imu"/>
</node>
</launch>
<launch>
<node ns="gps" pkg="nmea_gps_driver" type="nmea_gps_driver.py" name="nmea_gps" respawn="true">
<param name="port" value="/dev/clearpath/gps" />
<param name="baud" value="9600" />
<param name="useRMC" value="true" />
</node>
</launch>
<launch>
<group ns="teleop">
<include file="$(find clearpath_teleop)/teleop.launch">
<arg name="drive_speed" value="1.0" />
<arg name="turn_speed" value="1.0" />
<arg name="cmd_topic" value="/husky/cmd_vel" />
</include>
</group>
</launch>
/**
\mainpage
\htmlinclude manifest.html
\b husky_bringup is ...
<!--
Provide an overview of your package.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/
<package>
<description brief="husky_bringup">
This package provides a launch file to bring up the Husky
hardware interface in a way that will be accessible and
usable by the other software in husky_teleop and husky_kinect.
In the future, a script will be provided to allow this to
be controlled by the service command and launched on startup
of your machine.
</description>
<author>Mike Purvis, Ryan Gariepy</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/husky_bringup</url>
<depend package="rospy"/>
<depend package="sensor_msgs"/>
<depend package="nav_msgs"/>
<depend package="geometry_msgs"/>
<depend package="clearpath_bringup"/>
<depend package="clearpath_base"/>
<depend package="kdl"/>
<depend package="dynamic_reconfigure"/>
<depend package="diagnostic_msgs"/>
<depend package="app_manager"/>
</package>
#!/usr/bin/env python
import roslib; roslib.load_manifest('husky_bringup')
import roslib.rosenv
import rospy
import PyKDL
import copy
from math import sin,cos
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from clearpath_base.msg import Encoders
# Dynamic Reconfigure
import dynamic_reconfigure.server
from husky_bringup.cfg import HuskyConfig
ODOM_POSE_COVAR_MOTION = [1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e6]
ODOM_POSE_COVAR_NOMOVE = [1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9]
ODOM_TWIST_COVAR_MOTION = [1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e6]
ODOM_TWIST_COVAR_NOMOVE = [1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9]
class DeadReckoning(object):
def __init__(self):
rospy.init_node('dead_reckoning')
# Parameters
self.width = rospy.get_param('~width',0.5)
self.gyro_scale = rospy.get_param('~gyro_scale_correction',1.0)
# Set up publishers/subscribers
self.pub_imu = rospy.Publisher('imu_data',Imu)
self.pub_enc_odom = rospy.Publisher('encoder',Odometry);
rospy.Subscriber('imu/data', Imu, self.HandleIMU)
rospy.Subscriber('husky/data/encoders', Encoders, self.HandleEncoder)
# Initialize odometry message
self.odom = Odometry()
self.odom.header.frame_id = "odom_combined"
self.odom.child_frame_id = "base_footprint"
self.last_encoder = []
dynamic_reconfigure.server.Server(HuskyConfig, self.Reconfigure)
def Reconfigure(self, config, level):
# Handler for dynamic_reconfigure
self.gyro_scale = config['gyro_scale_correction']
return config
def HandleIMU(self,data):
# Correct IMU data
# Right now, gyro scale only
# TODO: Evaluate necessity of adding drift correction
imu_corr = copy.deepcopy(data)
imu_corr.header.frame_id = "base_link"
imu_corr.angular_velocity.z = data.angular_velocity.z * self.gyro_scale
self.pub_imu.publish(imu_corr)
def HandleEncoder(self,data):
# Initialize encoder state
if not self.last_encoder:
self.last_encoder = data
return
# Calculate deltas
dr = ((data.encoders[0].travel - self.last_encoder.encoders[0].travel) +
(data.encoders[1].travel - self.last_encoder.encoders[1].travel)) / 2;
da = ((data.encoders[1].travel - self.last_encoder.encoders[1].travel) -
(data.encoders[0].travel - self.last_encoder.encoders[0].travel)) / self.width;
ddr = (data.encoders[0].speed + data.encoders[1].speed)/2;
dda = (data.encoders[1].speed - data.encoders[0].speed)/self.width;
self.last_encoder = data
# Update data
o = self.odom.pose.pose.orientation
cur_heading = PyKDL.Rotation.Quaternion(o.x,o.y,o.z,o.w).GetEulerZYX()
self.odom.pose.pose.position.x += dr * cos(cur_heading[0])
self.odom.pose.pose.position.y += dr * sin(cur_heading[0])
quat = PyKDL.Rotation.RotZ(cur_heading[0] + da).GetQuaternion()
self.odom.pose.pose.orientation = Quaternion(quat[0],quat[1],quat[2],quat[3])
self.odom.twist.twist.linear.x = ddr
self.odom.twist.twist.angular.z = dda
self.odom.header.stamp = rospy.Time.now()
# If our wheels aren't moving, we're likely not moving at all.
# Adjust covariance appropriately
if data.encoders[0].speed == 0 and data.encoders[1].speed == 0:
self.odom.pose.covariance = ODOM_POSE_COVAR_NOMOVE
self.odom.twist.covariance = ODOM_TWIST_COVAR_NOMOVE
else:
self.odom.pose.covariance = ODOM_POSE_COVAR_MOTION
self.odom.twist.covariance = ODOM_TWIST_COVAR_MOTION
self.pub_enc_odom.publish(self.odom)
if __name__ == "__main__":
obj = DeadReckoning()
try:
rospy.spin()
except rospy.ROSInterruptException:
pass
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment