Skip to content
Snippets Groups Projects
Commit 296aba20 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a new application for the walk_client and action client. 

Added two server client to handle the walk_params and the servo_modules.
Deleted the old bioloid_action action message.
parent 11c4f9eb
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 533 additions and 59 deletions
bioloid_apps/CMakeLists.txt 0 → 100644
+ 181
− 0
View file @ 296aba20
cmake_minimum_required(VERSION 2.8.3)
project(bioloid_apps)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES bioloid_apps
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
## Declare a C++ library
# add_library(bioloid_apps
# src/${PROJECT_NAME}/bioloid_apps.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(bioloid_apps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
# add_executable(bioloid_apps_node src/bioloid_apps_node.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(bioloid_apps_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(bioloid_apps_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS bioloid_apps bioloid_apps_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_bioloid_apps.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
bioloid_apps/launch/action_client.launch 0 → 100644
+ 32
− 0
View file @ 296aba20
<launch>
<include file="$(find bioloid_description)/machines/bioloid.machines" />
<include file="$(find iri_bioloid_robot)/launch/bioloid.launch">
<arg name="config_file" value="bioloid_action.xml"/>
</include>
<group ns="bioloid">
<!-- launch the action client node -->
<node name="action_client"
pkg="action_client"
type="action_client"
output="screen"
machine="bioloid">
<remap from="/bioloid/motion"
to="/bioloid/robot/motion_action"/>
<remap from="/bioloid/action_client/set_servo_modules"
to="/bioloid/robot/set_servo_modules"/>
</node>
</group>
<!-- launch dynamic reconfigure -->
<!-- <node name="rqt_reconfigure"
pkg="rqt_reconfigure"
type="rqt_reconfigure"
respawn="false"
machine="humanoide"
output="screen"/>-->
</launch>
bioloid_apps/launch/action_client_sim.launch 0 → 100644
+ 23
− 0
View file @ 296aba20
<launch>
<include file="$(find bioloid_description)/launch/bioloid_sim.launch">
</include>
<!-- launch the action client node -->
<node name="action_client"
pkg="action_client"
type="action_client"
output="screen"
ns="/bioloid">
<remap from="/bioloid/motion"
to="/bioloid/robot/motion_action"/>
<remap from="/bioloid/action_client/set_servo_modules"
to="/bioloid/robot/set_servo_modules"/>
</node>
<!-- launch dynamic reconfigure -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
output="screen"/>
</launch>
bioloid_apps/launch/walk_client.launch 0 → 100644
+ 33
− 0
View file @ 296aba20
<launch>
<include file="$(find bioloid_description)/machines/bioloid.machines" />
<include file="$(find iri_bioloid_robot)/launch/bioloid.launch">
<arg name="config_file" value="bioloid_action.xml"/>
</include>
<group ns="bioloid">
<!-- launch the walk client node -->
<node name="walk_client"
pkg="walk_client"
type="walk_client"
output="screen"
machine="bioloid">
<remap from="/bioloid/walk_client/cmd_vel"
to="/bioloid/robot/cmd_vel"/>
<remap from="/bioloid/walk_client/set_walk_params"
to="/bioloid/robot/set_walk_params"/>
<remap from="/bioloid/walk_client/set_servo_modules"
to="/bioloid/robot/set_servo_modules"/>
</node>
</group>
<!-- launch dynamic reconfigure -->
<!-- <node name="rqt_reconfigure"
pkg="rqt_reconfigure"
type="rqt_reconfigure"
respawn="false"
machine="humanoide"
output="screen"/>-->
</launch>
bioloid_apps/launch/walk_client_sim.launch 0 → 100644
+ 24
− 0
View file @ 296aba20
<launch>
<include file="$(find bioloid_description)/launch/bioloid_sim.launch">
</include>
<!-- launch the walk client node -->
<node name="walk_client"
pkg="walk_client"
type="walk_client"
output="screen"
ns="/bioloid">
<remap from="/bioloid/walk_client/cmd_vel"
to="/bioloid/robot/cmd_vel"/>
<remap from="/bioloid/walk_client/set_walk_params"
to="/bioloid/robot/set_walk_params"/>
<remap from="/bioloid/walk_client/set_servo_modules"
to="/bioloid/robot/set_servo_modules"/>
</node>
<!-- launch dynamic reconfigure -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
output="screen"/>
</launch>
bioloid_apps/package.xml 0 → 100644
+ 50
− 0
View file @ 296aba20
<?xml version="1.0"?>
<package>
<name>bioloid_apps</name>
<version>0.0.0</version>
<description>The bioloid_apps package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="sergi@todo.todo">sergi</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/bioloid_apps</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
\ No newline at end of file
bioloid_control/config/bioloid_control.yaml
+ 18
− 18
View file @ 296aba20
......@@ -27,7 +27,7 @@ bioloid:
- j_ankle_roll_l
gains:
j_shoulder_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -35,7 +35,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_high_arm_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -43,7 +43,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_low_arm_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -51,7 +51,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_shoulder_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -59,7 +59,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_high_arm_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -67,7 +67,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_low_arm_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -75,7 +75,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_yaw_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -83,7 +83,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_roll_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -91,7 +91,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_pitch_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -99,7 +99,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_knee_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -107,7 +107,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_pitch_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -115,7 +115,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_roll_l:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -123,7 +123,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_yaw_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -131,7 +131,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_roll_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -139,7 +139,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_pelvis_pitch_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -147,7 +147,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_knee_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -155,7 +155,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_pitch_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......@@ -163,7 +163,7 @@ bioloid:
vel_limit: 6.0
effort_limit: 1.5
j_ankle_roll_r:
p: 5.0
p: 7.5
d: 0.0
i: 0.0
proxy:
......
bioloid_controller/CMakeLists.txt
+ 10
− 11
View file @ 296aba20
......@@ -4,7 +4,7 @@ project(bioloid_controller)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hardware_interface controller_manager xacro actionlib_msgs message_generation iri_action_server)
find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hardware_interface controller_manager xacro actionlib_msgs iri_action_server)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
......@@ -53,18 +53,17 @@ find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hard
# )
## Generate actions in the 'action' folder
add_action_files(
FILES
bioloid_motion.action
# add_action_files(
# FILES
# Action1.action
# Action2.action
)
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs actionlib_msgs # Or other packages containing msgs
)
# generate_messages(
# DEPENDENCIES
# std_msgs actionlib_msgs # Or other packages containing msgs
# )
###################################
## catkin specific configuration ##
......@@ -78,7 +77,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager xacro message_runtime actionlib_msgs iri_action_server
CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager xacro actionlib_msgs iri_action_server
# DEPENDS system_lib
)
......@@ -128,7 +127,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller.cpp
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
add_dependencies(bioloid_controller bioloid_controller_generate_messages_cpp)
add_dependencies(bioloid_controller humanoid_common_msgs_generate_messages_cpp)
## Specify libraries to link a library or executable target against
# target_link_libraries(bioloid_controller_node
......
bioloid_controller/action/bioloid_motion.action deleted 100644 → 0
+ 0
− 9
View file @ 11c4f9eb
#goal definition
int32 motion_id
---
#result definition
bool successful
---
#feedback
int32 current_page
int32 current_step
bioloid_controller/include/bioloid_controller.h
+ 26
− 14
View file @ 296aba20
......@@ -55,7 +55,10 @@
#include <control_toolbox/pid.h>
#include <iri_action_server/iri_action_server.h>
#include <bioloid_controller/bioloid_motionAction.h>
#include <humanoid_common_msgs/humanoid_motionAction.h>
#include <humanoid_common_msgs/set_servo_modules.h>
#include <humanoid_common_msgs/set_walk_params.h>
#include <humanoid_common_msgs/get_walk_params.h>
namespace bioloid_controller
{
......@@ -67,7 +70,6 @@ namespace bioloid_controller
class BioloidController : public controller_interface::Controller<HardwareInterface>
{
public:
BioloidController();
~BioloidController();
......@@ -76,18 +78,6 @@ namespace bioloid_controller
void stopping(const ros::Time& time);
void update(const ros::Time& time, const ros::Duration& period);
// action subscriber
IriActionServer<bioloid_controller::bioloid_motionAction> *motion_action_aserver_;
void motion_action_startCallback(const bioloid_controller::bioloid_motionGoalConstPtr& goal);
void motion_action_stopCallback(void);
bool motion_action_isFinishedCallback(void);
bool motion_action_hasSucceedCallback(void);
void motion_action_getResultCallback(bioloid_controller::bioloid_motionResultPtr& result);
void motion_action_getFeedbackCallback(bioloid_controller::bioloid_motionFeedbackPtr& feedback);
ros::Subscriber imu_sub;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
private:
typedef typename HardwareInterface::ResourceHandleType JointHandle;
typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
......@@ -107,6 +97,28 @@ namespace bioloid_controller
double balance_knee_left;
double balance_knee_right;
// motion action subscriber
IriActionServer<humanoid_common_msgs::humanoid_motionAction> *motion_action_aserver_;
void motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal);
void motion_action_stopCallback(void);
bool motion_action_isFinishedCallback(void);
bool motion_action_hasSucceedCallback(void);
void motion_action_getResultCallback(humanoid_common_msgs::humanoid_motionResultPtr& result);
void motion_action_getFeedbackCallback(humanoid_common_msgs::humanoid_motionFeedbackPtr& feedback);
/* services */
ros::ServiceServer set_servo_modules_server_;
bool set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res);
ros::ServiceServer set_walk_params_server_;
bool set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res);
ros::ServiceServer get_walk_params_server_;
bool get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res);
/* IMU subscriber */
ros::Subscriber imu_sub;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
// ROS API
ros::NodeHandle controller_nh_;
};
......
bioloid_controller/include/bioloid_controller_impl.h
+ 134
− 6
View file @ 296aba20
......@@ -18,6 +18,39 @@ extern double real_angles[PAGE_MAX_NUM_SERVOS];
// action module external variables
extern long int manager_current_angles[PAGE_MAX_NUM_SERVOS];
// joint names
const std::string servo_names[MANAGER_MAX_NUM_SERVOS]={std::string("Servo0"),
std::string("j_shoulder_pitch_r"),
std::string("j_shoulder_pitch_l"),
std::string("j_shoulder_roll_r"),
std::string("j_shoulder_roll_l"),
std::string("j_elbow_r"),
std::string("j_elbow_l"),
std::string("j_hip_yaw_r"),
std::string("j_hip_yaw_l"),
std::string("j_hip_roll_r"),
std::string("j_hip_roll_l"),
std::string("j_hip_pitch_r"),
std::string("j_hip_pitch_l"),
std::string("j_knee_r"),
std::string("j_knee_l"),
std::string("j_ankle_pitch_r"),
std::string("j_ankle_pitch_l"),
std::string("j_ankle_roll_r"),
std::string("j_ankle_roll_l"),
std::string("Servo19"),
std::string("Servo20"),
std::string("Servo21"),
std::string("Servo22"),
std::string("Servo23"),
std::string("Servo24"),
std::string("Servo25"),
std::string("Servo26"),
std::string("Servo27"),
std::string("Servo28"),
std::string("Servo29"),
std::string("Servo30")};
namespace bioloid_controller
{
namespace internal
......@@ -134,7 +167,8 @@ namespace bioloid_controller
// Cache controller node handle
controller_nh_ = controller_nh;
this->motion_action_aserver_=new IriActionServer<bioloid_controller::bioloid_motionAction>(root_nh,"robot/motion_action");
/* initialize actions */
this->motion_action_aserver_=new IriActionServer<humanoid_common_msgs::humanoid_motionAction>(root_nh,"robot/motion_action");
this->motion_action_aserver_->registerStartCallback(boost::bind(&BioloidController<HardwareInterface>::motion_action_startCallback, this, _1));
this->motion_action_aserver_->registerStopCallback(boost::bind(& BioloidController<HardwareInterface>::motion_action_stopCallback, this));
this->motion_action_aserver_->registerIsFinishedCallback(boost::bind(&BioloidController<HardwareInterface>::motion_action_isFinishedCallback, this));
......@@ -143,6 +177,11 @@ namespace bioloid_controller
this->motion_action_aserver_->registerGetFeedbackCallback(boost::bind(&BioloidController<HardwareInterface>::motion_action_getFeedbackCallback, this, _1));
this->motion_action_aserver_->start();
/* initialize services */
this->set_servo_modules_server_ = root_nh.advertiseService("robot/set_servo_modules", &BioloidController<HardwareInterface>::set_servo_modulesCallback, this);
this->set_walk_params_server_ = root_nh.advertiseService("robot/set_walk_params", &BioloidController<HardwareInterface>::set_walk_paramsCallback, this);
this->get_walk_params_server_ = root_nh.advertiseService("robot/get_walk_params", &BioloidController<HardwareInterface>::get_walk_paramsCallback, this);
// Controller name
name_ = getLeafNamespace(controller_nh_);
......@@ -207,8 +246,6 @@ namespace bioloid_controller
ram_init();
/* initialize motion modules */
manager_init(7800);// motion manager period in us
for(unsigned int i=0;i<MANAGER_MAX_NUM_SERVOS;++i)
manager_set_module(i,MM_WALKING);
std::cout << "motion manager initialized" << std::endl;
return true;
......@@ -234,7 +271,7 @@ namespace bioloid_controller
}
template <class HardwareInterface>
void BioloidController<HardwareInterface>::motion_action_startCallback(const bioloid_controller::bioloid_motionGoalConstPtr& goal)
void BioloidController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
{
std::cout << "Start action" << std::endl;
/* load a page and start execution */
......@@ -270,17 +307,108 @@ namespace bioloid_controller
}
template <class HardwareInterface>
void BioloidController<HardwareInterface>::motion_action_getResultCallback(bioloid_controller::bioloid_motionResultPtr& result)
void BioloidController<HardwareInterface>::motion_action_getResultCallback(humanoid_common_msgs::humanoid_motionResultPtr& result)
{
}
template <class HardwareInterface>
void BioloidController<HardwareInterface>::motion_action_getFeedbackCallback(bioloid_controller::bioloid_motionFeedbackPtr& feedback)
void BioloidController<HardwareInterface>::motion_action_getFeedbackCallback(humanoid_common_msgs::humanoid_motionFeedbackPtr& feedback)
{
feedback->current_page=action_get_current_page();
feedback->current_step=action_get_current_step();
}
template <class HardwareInterface>
bool BioloidController<HardwareInterface>::set_servo_modulesCallback(humanoid_common_msgs::set_servo_modules::Request &req, humanoid_common_msgs::set_servo_modules::Response &res)
{
unsigned int i=0,j=0;
for(i=0;i<req.names.size();i++)
{
for(j=0;j<MANAGER_MAX_NUM_SERVOS;j++)
{
if(req.names[i]==servo_names[j])
{
if(req.modules[i]=="action")
manager_set_module(j,MM_ACTION);
else if(req.modules[i]=="walking")
manager_set_module(j,MM_WALKING);
else if(req.modules[i]=="joints")
manager_set_module(j,MM_JOINTS);
else
manager_set_module(j,MM_NONE);
break;
}
}
}
res.ret=true;
return true;
}
template <class HardwareInterface>
bool BioloidController<HardwareInterface>::set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res)
{
ram_write_byte(BIOLOID_WALK_SWING_RIGHT_LEFT,(unsigned char)req.params.Y_SWAP_AMPLITUDE);
ram_write_byte(BIOLOID_WALK_SWING_TOP_DOWN,(unsigned char)req.params.Z_SWAP_AMPLITUDE);
ram_write_byte(BIOLOID_WALK_ARM_SWING_GAIN,(unsigned char)(req.params.ARM_SWING_GAIN*32.0));
ram_write_byte(BIOLOID_WALK_PELVIS_OFFSET,(unsigned char)(req.params.PELVIS_OFFSET*8.0));
ram_write_word(BIOLOID_WALK_HIP_PITCH_OFF_L,(unsigned short int)(req.params.HIP_PITCH_OFFSET*1024.0));
ram_write_byte(BIOLOID_WALK_X_OFFSET,(unsigned char)(req.params.X_OFFSET));
ram_write_byte(BIOLOID_WALK_Y_OFFSET,(unsigned char)(req.params.Y_OFFSET));
ram_write_byte(BIOLOID_WALK_Z_OFFSET,(unsigned char)(req.params.Z_OFFSET));
ram_write_byte(BIOLOID_WALK_YAW_OFFSET,(unsigned char)(req.params.A_OFFSET*8.0));
ram_write_byte(BIOLOID_WALK_PITCH_OFFSET,(unsigned char)(req.params.P_OFFSET*8.0));
ram_write_byte(BIOLOID_WALK_ROLL_OFFSET,(unsigned char)(req.params.R_OFFSET*8.0));
ram_write_word(BIOLOID_WALK_PERIOD_TIME_L,(unsigned short int)(req.params.PERIOD_TIME));
ram_write_byte(BIOLOID_WALK_DSP_RATIO,(unsigned char)(req.params.DSP_RATIO*256.0));
ram_write_byte(BIOLOID_WALK_STEP_FW_BW_RATIO,(unsigned char)(req.params.STEP_FB_RATIO*256.0));
ram_write_byte(BIOLOID_WALK_MAX_VEL,(unsigned char)(req.params.MAX_ACC));
ram_write_byte(BIOLOID_WALK_MAX_ROT_VEL,(unsigned char)(req.params.MAX_ROT_ACC*8.0));
res.ret=true;
}
template <class HardwareInterface>
bool BioloidController<HardwareInterface>::get_walk_paramsCallback(humanoid_common_msgs::get_walk_params::Request &req, humanoid_common_msgs::get_walk_params::Response &res)
{
unsigned char byte_value;
unsigned short int word_value;
ram_read_byte(BIOLOID_WALK_SWING_RIGHT_LEFT,&byte_value);
res.params.Y_SWAP_AMPLITUDE=byte_value;
ram_read_byte(BIOLOID_WALK_SWING_TOP_DOWN,&byte_value);
res.params.Z_SWAP_AMPLITUDE=byte_value;
ram_read_byte(BIOLOID_WALK_ARM_SWING_GAIN,&byte_value);
res.params.ARM_SWING_GAIN=((float)byte_value)/32.0;
ram_read_byte(BIOLOID_WALK_PELVIS_OFFSET,&byte_value);
res.params.PELVIS_OFFSET=((float)byte_value)/8.0;
ram_read_word(BIOLOID_WALK_HIP_PITCH_OFF_L,&word_value);
res.params.HIP_PITCH_OFFSET=((float)word_value)/1024.0;
ram_read_byte(BIOLOID_WALK_X_OFFSET,&byte_value);
res.params.X_OFFSET=byte_value;
ram_read_byte(BIOLOID_WALK_Y_OFFSET,&byte_value);
res.params.Y_OFFSET=byte_value;
ram_read_byte(BIOLOID_WALK_Z_OFFSET,&byte_value);
res.params.Z_OFFSET=byte_value;
ram_read_byte(BIOLOID_WALK_YAW_OFFSET,&byte_value);
res.params.A_OFFSET=((float)byte_value)/8.0;;
ram_read_byte(BIOLOID_WALK_PITCH_OFFSET,&byte_value);
res.params.P_OFFSET=((float)byte_value)/8.0;
ram_read_byte(BIOLOID_WALK_ROLL_OFFSET,&byte_value);
res.params.R_OFFSET=((float)byte_value)/8.0;
ram_read_word(BIOLOID_WALK_PERIOD_TIME_L,&word_value);
res.params.PERIOD_TIME=word_value;
ram_read_byte(BIOLOID_WALK_DSP_RATIO,&byte_value);
res.params.DSP_RATIO=((float)byte_value)/256.0;
ram_read_byte(BIOLOID_WALK_STEP_FW_BW_RATIO,&byte_value);
res.params.STEP_FB_RATIO=((float)byte_value)/256.0;
ram_read_byte(BIOLOID_WALK_MAX_VEL,&byte_value);
res.params.MAX_ACC=byte_value;
ram_read_byte(BIOLOID_WALK_MAX_ROT_VEL,&byte_value);
res.params.MAX_ROT_ACC=((float)byte_value)/8.0;
}
template <class HardwareInterface>
void BioloidController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
......
bioloid_controller/package.xml
+ 2
− 0
View file @ 296aba20
......@@ -46,6 +46,7 @@
<build_depend>iri_action_server</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_depend>humanoid_common_msgs</build_depend>
<build_depend>controller_interface</build_depend>
<build_depend>hardware_interface</build_depend>
......@@ -58,6 +59,7 @@
<run_depend>iri_action_server</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>actionlib_msgs</run_depend>
<run_depend>humanoid_common_msgs</run_depend>
<run_depend>controller_interface</run_depend>
<run_depend>hardware_interface</run_depend>
......
bioloid_robot/package.xml
+ 0
− 1
View file @ 296aba20
......@@ -45,7 +45,6 @@
<run_depend>bioloid_control</run_depend>
<run_depend>bioloid_controller</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
......
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