diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt
index 15594390e..5960b3ab1 100644
--- a/hrpsys_ros_bridge/CMakeLists.txt
+++ b/hrpsys_ros_bridge/CMakeLists.txt
@@ -76,7 +76,7 @@ if(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" )
endif(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" )
# define add_message_files before rtmbuild_init
-add_message_files(FILES MotorStates.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg)
+add_message_files(FILES MotorStates.msg LandingPosition.msg SteppableRegion.msg CogState.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg)
add_service_files(FILES SetSensorTransformation.srv)
# initialize rtmbuild
rtmbuild_init(geometry_msgs)
diff --git a/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l b/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l
index 8efe7a545..7ec5af22d 100644
--- a/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l
+++ b/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l
@@ -143,25 +143,25 @@
(send self :set-robot-state1 :auto-balancer-end-coords-list
(mapcar #'(lambda (x) (send robot x :end-coords :copy-worldcoords)) limb-list)))
;; Stabilizer parameter
- (let ((rsd (send self :parser-list "st_originRefCogVel")))
+ (let ((rsd (send self :parser-list "abc_originRefCogVel")))
(if rsd (send self :set-robot-state1 :stabilizer-reference-cogvel (scale 1e3 (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_originActCogVel")))
+ (let ((rsd (send self :parser-list "abc_originActCogVel")))
(if rsd (send self :set-robot-state1 :stabilizer-cogvel (scale 1e3 (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_originRefZmp")))
+ (let ((rsd (send self :parser-list "abc_originRefZmp")))
(if rsd (send self :set-robot-state1 :stabilizer-reference-zmp (scale 1e3 (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_originActZmp")))
+ (let ((rsd (send self :parser-list "abc_originActZmp")))
(if rsd (send self :set-robot-state1 :stabilizer-zmp (scale 1e3 (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_originNewZmp")))
+ (let ((rsd (send self :parser-list "abc_originNewZmp")))
(if rsd (send self :set-robot-state1 :stabilizer-newzmp (scale 1e3 (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_originRefCog")))
+ (let ((rsd (send self :parser-list "abc_originRefCog")))
(if rsd (send self :set-robot-state1 :stabilizer-reference-cog (scale 1e3 (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_originActCog")))
+ (let ((rsd (send self :parser-list "abc_originActCog")))
(if rsd (send self :set-robot-state1 :stabilizer-cog (scale 1e3 (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_actBaseRpy")))
+ (let ((rsd (send self :parser-list "abc_actBaseRpy")))
(if rsd (send self :set-robot-state1 :stabilizer-act-base-rpy (map float-vector #'rad2deg (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_currentBaseRpy")))
+ (let ((rsd (send self :parser-list "abc_currentBaseRpy")))
(if rsd (send self :set-robot-state1 :stabilizer-current-base-rpy (map float-vector #'rad2deg (send rsd :read-state)))))
- (let ((rsd (send self :parser-list "st_debugData")))
+ (let ((rsd (send self :parser-list "abc_debugData")))
(if rsd (send self :set-robot-state1 :stabilizer-debug-data (send rsd :read-state))))
;; Set stable rtc data
(send self :set-robot-state1
@@ -207,7 +207,9 @@
(send (send self :parser-list (format nil "rmfo_off_~A" (send f :name))) :read-state))
(send self :set-robot-state1
(read-from-string (format nil ":reference-~A" (string-downcase (send f :name))))
- (send (send self :parser-list (format nil "sh_~AOut" (send f :name))) :read-state))
+ (if (send self :parser-list (format nil "rfu_ref_~AOut" (send f :name)))
+ (send (send self :parser-list (format nil "rfu_ref_~AOut" (send f :name))) :read-state)
+ (send (send self :parser-list (format nil "sh_~AOut" (send f :name))) :read-state)))
)
;; (dolist (i (send robot :imu-sensors))
;; (send self :set-robot-state1
diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l
index 868112e93..1cd20c53b 100644
--- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l
+++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l
@@ -32,6 +32,12 @@
#'send self :rtmros-imu-callback :groupname groupname)
(ros::subscribe "/emergency_mode" std_msgs::Int32
#'send self :rtmros-emergency-mode-callback :groupname groupname)
+ (ros::subscribe "/is_stuck" std_msgs::Int32
+ #'send self :rtmros-is-stuck-callback :groupname groupname)
+ (ros::subscribe "/use_flywheel" std_msgs::Int32
+ #'send self :rtmros-use-flywheel-callback :groupname groupname)
+ (ros::subscribe "/estimated_fxy" geometry_msgs::PointStamped
+ #'send self :rtmros-estimated-fxy-callback :groupname groupname)
(mapcar #'(lambda (x)
(ros::subscribe (format nil "/~A_capture_point" x) geometry_msgs::PointStamped
#'send self :rtmros-capture-point-callback (read-from-string (format nil ":~A-capture-point" x)) :groupname groupname))
@@ -69,6 +75,20 @@
(let ((m (send msg :data)))
(send self :set-robot-state1 :emergency-mode m))
)
+ (:rtmros-is-stuck-callback
+ (msg)
+ (let ((m (send msg :data)))
+ (send self :set-robot-state1 :is-stuck m))
+ )
+ (:rtmros-use-flywheel-callback
+ (msg)
+ (let ((m (send msg :data)))
+ (send self :set-robot-state1 :use-flywheel m))
+ )
+ (:rtmros-estimated-fxy-callback
+ (msg)
+ (let ((p (send msg :point)))
+ (send self :set-robot-state1 :estimated-fxy (float-vector (send p :x) (send p :y) (send p :z)))))
(:rtmros-capture-point-callback
(ref-act msg)
(let ((p (send msg :point)))
@@ -117,13 +137,13 @@
(:reference-force-vector
(&optional (limb))
"Returns reference force-vector [N] list for all limbs obtained by :state.
- This value corresponds to StateHolder and SequencePlayer RTC.
+ This value corresponds to ReferenceForceUpdater, EmergencyStopper, StateHolder and SequencePlayer RTC prioritized in this order.
If a limb argument is specified, returns a vector for the limb."
(send self :tmp-force-moment-vector :force limb "reference"))
(:reference-moment-vector
(&optional (limb))
"Returns reference moment-vector [Nm] list for all limbs obtained by :state.
- This value corresponds to StateHolder and SequencePlayer RTC.
+ This value corresponds to ReferenceForceUpdater, EmergencyStopper, StateHolder and SequencePlayer RTC prioritized in this order.
If a limb argument is specified, returns a vector for the limb."
(send self :tmp-force-moment-vector :moment limb "reference"))
(:absolute-force-vector
@@ -1269,6 +1289,29 @@
"Call goPos with wait."
(send self :go-pos-no-wait xx yy th)
(send self :wait-foot-steps))
+ (:jump-to-no-wait
+ (xx yy zz ts tf)
+ "Call jumpTo without wait."
+ (send self :autobalancerservice_jumpTo :x xx :y yy :z zz :ts ts :tf tf))
+ (:jump-to
+ (xx yy zz ts tf)
+ "Call jumpTo with wait."
+ (send self :jump-to-no-wait xx yy zz ts tf)
+ (send self :wait-foot-steps))
+ (:go-pos-wheel
+ (xx yy th w-xx rv-max ra-max &optional (tm-off 0.0))
+ "Call goPosWheel without wait."
+ (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :rv_max rv-max :ra_max ra-max :tm_off tm-off)
+ (send self :wait-foot-steps))
+ (:go-wheel-no-wait
+ (xx rv-max ra-max)
+ "Call goWheel without wait."
+ (send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max))
+ (:go-wheel
+ (xx rv-max ra-max)
+ "Call goWheel with wait."
+ (send self :go-wheel-no-wait xx rv-max ra-max)
+ (send self :wait-foot-steps))
(:raw-get-foot-step-param
()
(send (send self :autobalancerservice_getfootstepparam) :i_param))
@@ -1359,6 +1402,48 @@
For arguments, please see :set-foot-steps-with-param-no-wait documentation."
(send self :set-foot-steps-with-param-no-wait foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list :overwrite-footstep-index overwrite-footstep-index)
(send self :wait-foot-steps))
+ (:set-foot-steps-with-wheel-no-wait
+ (foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list &key (overwrite-footstep-index 0))
+ (unless (listp (car foot-step-list))
+ (setq foot-step-list (mapcar #'(lambda (fs) (list fs)) foot-step-list)
+ step-height-list (mapcar #'(lambda (sh) (list sh)) step-height-list)
+ step-time-list (mapcar #'(lambda (st) (list st)) step-time-list)
+ toe-angle-list (mapcar #'(lambda (ta) (list ta)) toe-angle-list)
+ heel-angle-list (mapcar #'(lambda (ha) (list ha)) heel-angle-list)
+ goal-pos-list (mapcar #'(lambda (gp) (list gp)) goal-pos-list)
+ rv-max-list (mapcar #'(lambda (rv) (list rv)) rv-max-list)
+ ra-max-list (mapcar #'(lambda (ra) (list ra)) ra-max-list)))
+ (send self :autobalancerservice_setfootstepswithwheel
+ :fss
+ (mapcar #'(lambda (fs)
+ (instance hrpsys_ros_bridge::openhrp_autobalancerservice_footsteps :init
+ :fs
+ (mapcar #'(lambda (f)
+ (send self :eus-footstep->abc-footstep f))
+ fs)))
+ foot-step-list)
+ :spss
+ (mapcar #'(lambda (shs sts tas has)
+ (instance hrpsys_ros_bridge::openhrp_autobalancerservice_stepparams :init
+ :sps
+ (mapcar #'(lambda (sh st ta ha)
+ (instance hrpsys_ros_bridge::openhrp_autobalancerservice_stepparam :init :step_height (* sh 1e-3) :step_time st :toe_angle ta :heel_angle ha))
+ shs sts tas has)))
+ step-height-list step-time-list toe-angle-list heel-angle-list)
+ :wpss
+ (mapcar #'(lambda (gps rvs ras)
+ (instance hrpsys_ros_bridge::openhrp_autobalancerservice_wheelparams :init
+ :wps
+ (mapcar #'(lambda (gp rv ra)
+ (instance hrpsys_ros_bridge::openhrp_autobalancerservice_wheelparam :init :goal_pos (* gp 1e-3) :rv_max rv :ra_max ra))
+ gps rvs ras)))
+ goal-pos-list rv-max-list ra-max-list)
+ :overwrite_fs_idx overwrite-footstep-index
+ ))
+ (:set-foot-steps-with-wheel
+ (foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list &key (overwrite-footstep-index 0))
+ (send self :set-foot-steps-with-wheel-no-wait foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list :overwrite-footstep-index overwrite-footstep-index)
+ (send self :wait-foot-steps))
(:set-foot-steps-roll-pitch
(angle &key (axis :x))
"Set foot steps with roll or pitch orientation.
@@ -1752,9 +1837,9 @@
;; StabilizerService
(def-set-get-param-method
- 'hrpsys_ros_bridge::Openhrp_StabilizerService_stParam
+ 'hrpsys_ros_bridge::Openhrp_AutoBalancerService_StabilizerParam
:raw-set-st-param :get-st-param :get-st-param-arguments
- :stabilizerservice_setparameter :stabilizerservice_getparameter)
+ :autobalancerservice_setstabilizerparam :autobalancerservice_getstabilizerparam)
(defmethod rtm-ros-robot-interface
(:set-st-param
@@ -1830,7 +1915,7 @@
"Set Stabilzier parameter by default parameters."
(unless (send self :get :default-st-param)
(send self :put :default-st-param (send self :get-st-param)))
- (send self :stabilizerservice_setparameter :i_param (send self :get :default-st-param))
+ (send self :autobalancerservice_setparameter :i_param (send self :get :default-st-param))
)
(:set-st-param-by-ratio
(state-feedback-gain-ratio
@@ -1876,12 +1961,12 @@
(:start-st
()
"Start Stabilizer Mode."
- (send self :stabilizerservice_startstabilizer)
+ (send self :autobalancerservice_startstabilizer)
)
(:stop-st
()
"Stop Stabilizer Mode."
- (send self :stabilizerservice_stopstabilizer)
+ (send self :autobalancerservice_stopstabilizer)
)
)
@@ -2204,9 +2289,9 @@
(print-ros-msg (send self :get-gait-generator-param))
(format t ";; :get-auto-balancer-param~%")
(print-ros-msg (send self :get-auto-balancer-param)))
- (when (find "/StabilizerServiceROSBridge" rosbridge-nodes :test #'string=)
- (format t ";; :get-st-param~%")
- (print-ros-msg (send self :get-st-param)))
+ ;; (when (find "/StabilizerServiceROSBridge" rosbridge-nodes :test #'string=)
+ ;; (format t ";; :get-st-param~%")
+ ;; (print-ros-msg (send self :get-st-param)))
(when (find "/ObjectContactTurnaroundDetectorServiceROSBridge" rosbridge-nodes :test #'string=)
(format t ";; :get-object-turnaround-detector-param~%")
(print-ros-msg (send self :get-object-turnaround-detector-param)))
diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
index 6963b96ac..2d2932b6e 100644
--- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
+++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
@@ -161,14 +161,18 @@
-
-
-
-
+
+
+
+
-
+
-
+
+
+
+
+
@@ -260,6 +264,10 @@
+
+
+
+
diff --git a/hrpsys_ros_bridge/msg/CogState.msg b/hrpsys_ros_bridge/msg/CogState.msg
new file mode 100644
index 000000000..f47603f41
--- /dev/null
+++ b/hrpsys_ros_bridge/msg/CogState.msg
@@ -0,0 +1,9 @@
+Header header
+
+float64 x
+float64 y
+float64 z
+float64 vx
+float64 vy
+float64 vz
+int8 l_r
diff --git a/hrpsys_ros_bridge/msg/LandingPosition.msg b/hrpsys_ros_bridge/msg/LandingPosition.msg
new file mode 100644
index 000000000..61ec83856
--- /dev/null
+++ b/hrpsys_ros_bridge/msg/LandingPosition.msg
@@ -0,0 +1,9 @@
+Header header
+
+float64 x
+float64 y
+float64 z
+float64 nx
+float64 ny
+float64 nz
+int8 l_r
diff --git a/hrpsys_ros_bridge/msg/SteppableRegion.msg b/hrpsys_ros_bridge/msg/SteppableRegion.msg
new file mode 100644
index 000000000..862cd4ec2
--- /dev/null
+++ b/hrpsys_ros_bridge/msg/SteppableRegion.msg
@@ -0,0 +1,5 @@
+Header header
+geometry_msgs/PolygonStamped[] polygons
+uint32[] labels
+float32[] likelihood
+uint32 l_r
\ No newline at end of file
diff --git a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py
index c701fdc80..586684d09 100755
--- a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py
+++ b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py
@@ -28,12 +28,12 @@ def connecSensorRosBridgePort(url, rh, bridge, vs, rmfo, sh, es, rfu, subscripti
print program_name, "connect ", sen.name, rmfo.port("off_" + sen.name).get_port_profile().name, bridge.port("off_" + sen.name).get_port_profile().name
connectPorts(rmfo.port("off_" + sen.name), bridge.port("off_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) # for abs forces
if sen.type == 'Force' and bridge.port("ref_" + sen.name): # for reference forces
- if rfu != None and rfu.port("ref_"+sen.name+"Out"):
- print program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
- connectPorts(rfu.port("ref_"+sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
- elif es != None and es.port(sen.name+"Out"):
+ if es != None and es.port(sen.name+"Out"):
print program_name, "connect ", sen.name, es.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
connectPorts(es.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
+ elif rfu != None and rfu.port("ref_"+sen.name+"Out"):
+ print program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
+ connectPorts(rfu.port("ref_"+sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
elif sh.port(sen.name+"Out"):
print program_name, "connect ", sen.name, sh.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name
connectPorts(sh.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy)
@@ -78,4 +78,3 @@ def initSensorRosBridgeConnection(url, simulator_name, rosbridge_name, managerho
initSensorRosBridgeConnection(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4], sys.argv[5], sys.argv[6], sys.argv[7])
else :
print program_name, " requires url, simulator_name, rosbridge_name"
-
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index bebd13460..27702bf5c 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -62,6 +62,8 @@ HrpsysSeqStateROSBridge::HrpsysSeqStateROSBridge(RTC::Manager* manager) :
joint_controller_state_pub = nh.advertise("/fullbody_controller/state", 1);
#endif
trajectory_command_sub = nh.subscribe("/fullbody_controller/command", 1, &HrpsysSeqStateROSBridge::onTrajectoryCommandCB, this);
+ landing_height_sub = nh.subscribe("/landing_height", 1, &HrpsysSeqStateROSBridge::onLandingHeightCB, this);
+ steppable_region_sub = nh.subscribe("/steppable_region", 1, &HrpsysSeqStateROSBridge::onSteppableRegionCB, this);
mot_states_pub = nh.advertise("/motor_states", 1);
odom_pub = nh.advertise("/odom", 1);
imu_pub = nh.advertise("/imu", 1);
@@ -158,12 +160,18 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() {
ref_contact_states_pub = nh.advertise("/ref_contact_states", 10);
act_contact_states_pub = nh.advertise("/act_contact_states", 10);
cop_pub.resize(m_mcforceName.size());
+ landing_target_pub = nh.advertise("/landing_target", 10);
+ end_cog_state_pub = nh.advertise("/end_cog_state", 10);
for (unsigned int i=0; i(tmpname+"_cop", 10);
}
em_mode_pub = nh.advertise("emergency_mode", 10);
+ is_stuck_pub = nh.advertise("is_stuck", 10);
+ use_flywheel_pub = nh.advertise("use_flywheel", 10);
+ estimated_fxy_pub = nh.advertise("estimated_fxy", 10);
+ current_steppable_region_pub = nh.advertise("current_steppable_region", 10);
return RTC::RTC_OK;
}
@@ -183,6 +191,13 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
std::vector joint_names = trajectory.joint_names;
+ // set body to current reference angle
+ if ( m_mcangle.data.length() == body->joints().size() ) {
+ for (unsigned int i = 0; i < body->joints().size(); i++){
+ body->joint(i)->q = m_mcangle.data[i];
+ }
+ }
+
for (unsigned int i=0; i < trajectory.points.size(); i++) {
angles[i].length(body->joints().size());
@@ -275,6 +290,33 @@ void HrpsysSeqStateROSBridge::onTrajectoryCommandCB(const trajectory_msgs::Joint
onJointTrajectory(*msg);
}
+void HrpsysSeqStateROSBridge::onLandingHeightCB(const hrpsys_ros_bridge::LandingPosition::ConstPtr& msg) {
+ m_rslandingHeight.data.x = msg->x;
+ m_rslandingHeight.data.y = msg->y;
+ m_rslandingHeight.data.z = msg->z;
+ m_rslandingHeight.data.nx = msg->nx;
+ m_rslandingHeight.data.ny = msg->ny;
+ m_rslandingHeight.data.nz = msg->nz;
+ m_rslandingHeight.data.l_r = msg->l_r;
+ m_rslandingHeightOut.write();
+}
+
+void HrpsysSeqStateROSBridge::onSteppableRegionCB(const hrpsys_ros_bridge::SteppableRegion::ConstPtr& msg) {
+ size_t convex_num(msg->polygons.size());
+ m_rssteppableRegion.data.region.length(convex_num);
+ for (size_t i = 0; i < convex_num; i++) {
+ size_t vs_num(msg->polygons[i].polygon.points.size());
+ m_rssteppableRegion.data.region[i].length(3 * vs_num); // x,y,z components
+ for (size_t j = 0; j < vs_num; j++) {
+ m_rssteppableRegion.data.region[i][3*j] = msg->polygons[i].polygon.points[j].x;
+ m_rssteppableRegion.data.region[i][3*j+1] = msg->polygons[i].polygon.points[j].y;
+ m_rssteppableRegion.data.region[i][3*j+2] = msg->polygons[i].polygon.points[j].z;
+ }
+ }
+ m_rssteppableRegion.data.l_r = msg->l_r;
+ m_rssteppableRegionOut.write();
+}
+
bool HrpsysSeqStateROSBridge::setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req,
hrpsys_ros_bridge::SetSensorTransformation::Response& res)
{
@@ -797,6 +839,53 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
}
}
+ if ( m_rslandingTargetIn.isNew() ) {
+ try {
+ m_rslandingTargetIn.read();
+ hrpsys_ros_bridge::LandingPosition landingTargetv;
+ if ( use_hrpsys_time ) {
+ landingTargetv.header.stamp = ros::Time(m_rslandingTarget.tm.sec, m_rslandingTarget.tm.nsec);
+ }else{
+ landingTargetv.header.stamp = tm_on_execute;
+ }
+ landingTargetv.header.frame_id = rootlink_name;
+ landingTargetv.x = m_rslandingTarget.data.x;
+ landingTargetv.y = m_rslandingTarget.data.y;
+ landingTargetv.z = m_rslandingTarget.data.z;
+ landingTargetv.l_r = m_rslandingTarget.data.l_r;
+ landing_target_pub.publish(landingTargetv);
+ }
+ catch(const std::runtime_error &e)
+ {
+ ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
+ }
+ }
+
+ if ( m_rsendCogStateIn.isNew() ) {
+ try {
+ m_rsendCogStateIn.read();
+ hrpsys_ros_bridge::CogState endCogStatev;
+ if ( use_hrpsys_time ) {
+ endCogStatev.header.stamp = ros::Time(m_rsendCogState.tm.sec, m_rsendCogState.tm.nsec);
+ }else{
+ endCogStatev.header.stamp = tm_on_execute;
+ }
+ endCogStatev.header.frame_id = rootlink_name;
+ endCogStatev.x = m_rsendCogState.data.x;
+ endCogStatev.y = m_rsendCogState.data.y;
+ endCogStatev.z = m_rsendCogState.data.z;
+ endCogStatev.vx = m_rsendCogState.data.vx;
+ endCogStatev.vy = m_rsendCogState.data.vy;
+ endCogStatev.vz = m_rsendCogState.data.vz;
+ endCogStatev.l_r = m_rsendCogState.data.l_r;
+ end_cog_state_pub.publish(endCogStatev);
+ }
+ catch(const std::runtime_error &e)
+ {
+ ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
+ }
+ }
+
if ( m_refContactStatesIn.isNew() && m_controlSwingSupportTimeIn.isNew() ) {
try {
m_refContactStatesIn.read();
@@ -902,6 +991,81 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
}
}
+ if ( m_isStuckIn.isNew() ) {
+ try {
+ m_isStuckIn.read();
+ std_msgs::Int32 is_stuck;
+ is_stuck.data = m_isStuck.data;
+ is_stuck_pub.publish(is_stuck);
+ }
+ catch(const std::runtime_error &e)
+ {
+ ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
+ }
+ }
+
+ if ( m_useFlywheelIn.isNew() ) {
+ try {
+ m_useFlywheelIn.read();
+ std_msgs::Int32 use_flywheel;
+ use_flywheel.data = m_useFlywheel.data;
+ use_flywheel_pub.publish(use_flywheel);
+ }
+ catch(const std::runtime_error &e)
+ {
+ ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
+ }
+ }
+
+ if ( m_estimatedFxyIn.isNew() ) {
+ try {
+ m_estimatedFxyIn.read();
+ geometry_msgs::PointStamped fxyv;
+ if ( use_hrpsys_time ) {
+ fxyv.header.stamp = ros::Time(m_estimatedFxy.tm.sec, m_estimatedFxy.tm.nsec);
+ }else{
+ fxyv.header.stamp = tm_on_execute;
+ }
+ fxyv.header.frame_id = rootlink_name;
+ fxyv.point.x = m_estimatedFxy.data.x;
+ fxyv.point.y = m_estimatedFxy.data.y;
+ fxyv.point.z = m_estimatedFxy.data.z;
+ estimated_fxy_pub.publish(fxyv);
+ }
+ catch(const std::runtime_error &e)
+ {
+ ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
+ }
+ }
+
+ if ( m_currentSteppableRegionIn.isNew() ) {
+ try {
+ m_currentSteppableRegionIn.read();
+ hrpsys_ros_bridge::SteppableRegion region;
+ bool is_valid = false;
+ size_t convex_num(m_currentSteppableRegion.data.region.length());
+ region.polygons.resize(convex_num);
+ for (size_t i = 0; i < convex_num; i++) {
+ size_t vs_num(m_currentSteppableRegion.data.region[i].length()/3);
+ if (vs_num >= 3) is_valid = true;
+ region.polygons[i].polygon.points.resize(vs_num);
+ for (size_t j = 0; j < vs_num; j++) {
+ region.polygons[i].polygon.points[j].x = m_currentSteppableRegion.data.region[i][3*j];
+ region.polygons[i].polygon.points[j].y = m_currentSteppableRegion.data.region[i][3*j+1];
+ region.polygons[i].polygon.points[j].z = m_currentSteppableRegion.data.region[i][3*j+2];
+ }
+ }
+ region.l_r = m_currentSteppableRegion.data.l_r;
+ if (is_valid) current_steppable_region_pub.publish(region);
+ }
+ catch(const std::runtime_error &e)
+ {
+ ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
+ }
+ }
+
+
+
//
return RTC::RTC_OK;
}
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
index 4068ca1a3..a7a907312 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -30,6 +30,9 @@
#include "dynamic_reconfigure/Reconfigure.h"
#include "hrpsys_ros_bridge/MotorStates.h"
#include "hrpsys_ros_bridge/ContactStatesStamped.h"
+#include "hrpsys_ros_bridge/LandingPosition.h"
+#include "hrpsys_ros_bridge/CogState.h"
+#include "hrpsys_ros_bridge/SteppableRegion.h"
#include "diagnostic_msgs/DiagnosticArray.h"
#include "sensor_msgs/Imu.h"
#include "hrpsys_ros_bridge/SetSensorTransformation.h"
@@ -54,14 +57,16 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl
void onFollowJointTrajectoryActionGoal();
void onFollowJointTrajectoryActionPreempt();
void onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg);
+ void onLandingHeightCB(const hrpsys_ros_bridge::LandingPosition::ConstPtr& msg);
+ void onSteppableRegionCB(const hrpsys_ros_bridge::SteppableRegion::ConstPtr& msg);
bool sendMsg (dynamic_reconfigure::Reconfigure::Request &req,
dynamic_reconfigure::Reconfigure::Response &res);
bool setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req,
hrpsys_ros_bridge::SetSensorTransformation::Response& res);
private:
ros::NodeHandle nh;
- ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub;
- ros::Subscriber trajectory_command_sub;
+ ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub, use_flywheel_pub, estimated_fxy_pub, current_steppable_region_pub;
+ ros::Subscriber trajectory_command_sub, landing_height_sub, steppable_region_sub;
std::vector fsensor_pub, cop_pub;
#ifdef USE_PR2_CONTROLLERS_MSGS
actionlib::SimpleActionServer joint_trajectory_server;
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
index e59303ba4..3c53b537f 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
@@ -41,10 +41,18 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager)
m_rsactCPIn("rsactCapturePoint", m_rsactCP),
m_rsCOPInfoIn("rsCOPInfo", m_rsCOPInfo),
m_emergencyModeIn("emergencyMode", m_emergencyMode),
+ m_isStuckIn("isStuck", m_isStuck),
+ m_useFlywheelIn("useFlywheel", m_useFlywheel),
+ m_estimatedFxyIn("estimatedFxy", m_estimatedFxy),
m_refContactStatesIn("refContactStates", m_refContactStates),
m_actContactStatesIn("actContactStates", m_actContactStates),
m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime),
m_mctorqueOut("mctorque", m_mctorque),
+ m_rslandingTargetIn("rslandingTarget", m_rslandingTarget),
+ m_rsendCogStateIn("rsendCogState", m_rsendCogState),
+ m_rslandingHeightOut("rslandingHeight", m_rslandingHeight),
+ m_rssteppableRegionOut("rssteppableRegion", m_rssteppableRegion),
+ m_currentSteppableRegionIn("currentSteppableRegion", m_currentSteppableRegion),
m_SequencePlayerServicePort("SequencePlayerService")
//
@@ -73,12 +81,20 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize()
addInPort("servoState", m_servoStateIn);
addInPort("rsCOPInfo", m_rsCOPInfoIn);
addInPort("emergencyMode", m_emergencyModeIn);
+ addInPort("isStuck", m_isStuckIn);
+ addInPort("useFlywheel", m_useFlywheelIn);
+ addInPort("estimatedFxy", m_estimatedFxyIn);
addInPort("refContactStates", m_refContactStatesIn);
addInPort("actContactStates", m_actContactStatesIn);
addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn);
+ addInPort("rslandingTarget", m_rslandingTargetIn);
+ addInPort("rsendCogState", m_rsendCogStateIn);
+ addInPort("currentSteppableRegion", m_currentSteppableRegionIn);
// Set OutPort buffer
addOutPort("mctorque", m_mctorqueOut);
+ addOutPort("rslandingHeight", m_rslandingHeightOut);
+ addOutPort("rssteppableRegion", m_rssteppableRegionOut);
// Set service provider to Ports
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
index 20c5dfb82..44472a201 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
@@ -153,10 +153,22 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase
InPort m_rsCOPInfoIn;
TimedLong m_emergencyMode;
InPort m_emergencyModeIn;
+ TimedBoolean m_isStuck;
+ InPort m_isStuckIn;
+ TimedBoolean m_useFlywheel;
+ InPort m_useFlywheelIn;
+ TimedPoint3D m_estimatedFxy;
+ InPort m_estimatedFxyIn;
TimedBooleanSeq m_refContactStates, m_actContactStates;
InPort m_refContactStatesIn, m_actContactStatesIn;
TimedDoubleSeq m_controlSwingSupportTime;
InPort m_controlSwingSupportTimeIn;
+ OpenHRP::TimedLandingPosition m_rslandingTarget;
+ InPort m_rslandingTargetIn;
+ OpenHRP::TimedCogState m_rsendCogState;
+ InPort m_rsendCogStateIn;
+ OpenHRP::TimedSteppableRegion m_currentSteppableRegion;
+ InPort m_currentSteppableRegionIn;
//
@@ -164,6 +176,10 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase
//
TimedDoubleSeq m_mctorque;
OutPort m_mctorqueOut;
+ OpenHRP::TimedLandingPosition m_rslandingHeight;
+ OutPort m_rslandingHeightOut;
+ OpenHRP::TimedSteppableRegion m_rssteppableRegion;
+ OutPort m_rssteppableRegionOut;
//
diff --git a/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py b/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py
index b10bbbcd2..1e06df221 100755
--- a/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py
+++ b/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py
@@ -30,25 +30,27 @@ def setStAbcParameters (self):
tlp.debug_print_freq = int(10 / 0.002)
tlp.alarmRatio = 1.0
self.tl_svc.setParameter(tlp)
+ abcp=self.abc_svc.getAutoBalancerParam()[1]
+ abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY
# ST parameters
- stp=self.st_svc.getParameter()
- stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP
+ stp=self.abc_svc.getStabilizerParam()
+ stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP
# eefm st params
tmp_leg_inside_margin=71.12*1e-3
tmp_leg_outside_margin=71.12*1e-3
tmp_leg_front_margin=182.0*1e-3
tmp_leg_rear_margin=72.0*1e-3
- rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
- OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
- OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
- OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
- lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
- OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
- OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
- OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
+ rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]),
+ OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]),
+ OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]),
+ OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])]
+ lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]),
+ OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]),
+ OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]),
+ OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])]
rarm_vertices = rleg_vertices
larm_vertices = lleg_vertices
- stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
+ stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices])
stp.eefm_leg_inside_margin=tmp_leg_inside_margin
stp.eefm_leg_outside_margin=tmp_leg_outside_margin
stp.eefm_leg_front_margin=tmp_leg_front_margin
@@ -63,7 +65,30 @@ def setStAbcParameters (self):
stp.k_tpcc_x=[4.0, 4.0]
stp.k_brot_p=[0.0, 0.0]
stp.k_brot_tc=[0.1, 0.1]
- self.st_svc.setParameter(stp)
+ self.abc_svc.setStabilizerParam(stp)
+ gg=self.abc_svc.getGaitGeneratorParam()[1]
+ gg.default_step_time=1.0
+ gg.default_step_height=0.05
+ gg.default_double_support_ratio=0.15
+ gg.swing_trajectory_delay_time_offset=0.15
+ gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0]
+ gg.swing_trajectory_final_distance_weight=3.0
+ gg.leg_margin=[0.18, 0.07, 0.07, 0.07]
+ gg.safe_leg_margin=[0.15, 0.05, 0.05, 0.05]
+ gg.stride_limitation_for_circle_type=[0.15, 0.3, 15, 0.1, 0.13]
+ gg.overwritable_stride_limitation=[0.35, 0.4, 0, 0.35, 0.12]
+ gg.margin_time_ratio=0.3
+ gg.use_act_states=False
+ gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE
+ gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE
+ gg.toe_pos_offset_x = 1e-3*117.338;
+ gg.heel_pos_offset_x = 1e-3*-116.342;
+ gg.toe_zmp_offset_x = 1e-3*117.338;
+ gg.heel_zmp_offset_x = 1e-3*-116.342;
+ gg.optional_go_pos_finalize_footstep_num=1
+ gg.overwritable_footstep_index_offset=1
+ self.abc_svc.setGaitGeneratorParam(gg)
+
def __init__(self, robotname=""):
HrpsysConfigurator.__init__(self)